# Adϕ-Strategy: Facilitating Dual-Formation Control of a Virtually Connected Team

Authors: Bibhya Sharma; Jito Vanualailai; Avinesh Prasad
Journal: Journal of Advanced Transportation
 (2017)

---

## Abstract

This paper describes the design of new centralized acceleration-based controllers for the multitask problem of motion planning and control of a coordinated lead-carrier team fixed in adual-formation within an obstacle-ridden environment. A dϕ-strategy, where d and ϕ are Euclidean measures with respect to the lead robot, is developed to ensure virtual connectivity of the carrier robots to the lead robot. This connectivity, built into the system itself, inherently ensures globally rigid formation between each lead-carrier pair of the team. Moreover, a combination of target configuration, dϕ-strategy, orientation consensus, and avoidance of end-effector of robots results in a second, locally rigid formation (not infinitesimally rigid). Therefore, for the first time, a dual-formation control problem of a lead-carrier team of mobile manipulators is considered. This and other kinodynamic constraints have been treated simultaneously via the overarching Lyapunov-based control scheme, essentially a potential field method favored in the field of robotics. The formulation of this new scheme, demonstrated effectively via computer simulations, is timely, given that the current proposed engineering solutions, allowing autonomous vehicles on public roads, include the development of special lanes imbued with special sensors and wireless technologies.


---

## Body

## 1. Introduction

### 1.1. Motivational Work

Researchers have continually designed and introduced numerous robotic systems that have a large array of possibilities for real world applications. One such system of increasing practical importance is based on swarm intelligence, which is a coordinated robot collective that is frequently sighted in airports, factories, wharfs, farms, and mines [1] and will play a huge role in the future as well. Swarms or flocks in nature are commonly dovetailed with formation types and unique patterns, seen, for example, in schools of fishes, flocks of birds, swarms of insects, and herds of animals. In addition, a wide spectrum of the “formation rigidity” has also appeared in literature using the nomenclature from [2, 3]; on one end there aresplit/rejoin maneuvers also known as minimally rigid formations [2, 3] which are required in applications such as reconnaissance, sampling, and surveillance, while on the other end there areglobally rigid formations which are required in applications that require cooperative payload transportation [4–6]. Then there arelocally rigid formations (required in convoying and demining [1, 7]) which are not infinitesimally rigid and allow for slight distortions temporarily. Adual-formation would be accomplishing two different formation types or patterns in parallel from a team in motion. However, it is noted that motion planning and control of a dual-formation are usually avoided due to the complexity of the algorithms and associated computer simulations and roll out to experimental designs.Formation control can be defined as controlling the configuration or state of a team of agents en route to a target, normally maintaining constant their relative locations, hence maintaining the formation type and pattern [8–11]. Indeed, scalable formations with increased heterogeneity are evidently deployed to satisfy stringent time, labor, and cost demands. The literature contains rich and diverse research on motion planning and control of such collectives, which are ideally suited and capable of performing wide-ranging tasks such as those mentioned above. These can take place in known or partially known environments; an environment may be harsh or hazardous or even inaccessible to humans [1]. Collectives provide increased performance, redundancy, flexibility, robustness, and multibehaviors which is difficult, and at times impossible, with single agents [12–14].A number of approaches such as leader-follower, virtual structure, nearest neighbors, social potentials, behavior-based, and formation-constrained functions have been utilized to address formation control [11]. While the lead-carrier approach is known for its poor disturbance rejection properties [15] and dependence placed upon a single agent, it is commonly utilized in literature because of its simplicity, scalability, and ability to contain an array of formations with richer specifications and complexities [3, 9, 11, 15–17]. This work deploys a new lead-carrier scheme to establish and control a team fixed in a dual-formation, for the first time.


### 1.2. Mechanical System

Of the many mechanical systems recorded in literature, mobile manipulators play a pivotal role in most industries that require a certain degree of automation and repetition. The literature takes into account the type of mobility of mobile manipulators with four possible configurations: (1)Type(h,h) where both platform and manipulator contain holonomic constraints, (2)Type(h,nh) where the platform is holonomic and manipulator is nonholonomic, (3)Type(nh,h) where the platform is nonholonomic but the manipulator is holonomic, and (4)Type(nh,nh) where both the platform and the manipulator contain nonholonomic constraints.In this paper we considerType(nh,h) which was introduced by Seraji in [18] in 1998 and classified as thenonholonomic mobile manipulators. Since then the literature has grown with research usingType(nh,h) mobile manipulators (see, e.g., [11, 19–21]). The challenges associated with motion planning and control of theType(nh,h) are amplified by the intimate coupling of the nonholonomic and holonomic constraints arising from the amalgamation of an articulated robotic arm and a wheeled platform.


### 1.3. Lyapunov-Based Control Scheme

The Lyapunov-based control scheme (LBCS), proposed by Sharma et al. in [3, 11], can be categorized under the artificial potential field method commonly deployed in the area of robotics research, especially applied to motion planning and control of various robotic systems [1, 3, 11]. We utilize the control scheme to derive centralized acceleration-based controllers for a dual-formation control of a team of lead-carrier 2-link mobile manipulators.The seminal idea behind LbCS is to design an appropriate Lyapunov function which acts astotal potentials. LbCS entails the construction of attractive and obstacle avoidance functions for the attraction to target and repulsion from various obstacles, respectively. While the attractive functions can be treated as attractive potential field functions, per se, the repulsive potential field functions are designed as ratios with the obstacle avoidance function in the denominator and a positivetuning parameter in the numerator. The sum of these potential functions is termed as thetotal potentials, a basis to design the controllers for the team. The governing principle behind the method is to attach attractive field to the target and repulsive field to each obstacle. The whole workspace is then inundated with positive and negative fields, with the direction of motion facilitated via the notion of steepest descent. For the vehicular systems, the gradient of the total potentials, referred to as the input force, determines the speed and the direction along which the vehicle moves.


### 1.4. Contributions

The Lyapunov-based control scheme utilized in this paper has been introduced in numerous applications for various robotic systems including ones tagged with holonomic or nonholonomic constraints [3, 22]. It is relatively easier to construct mathematical functions from limitations, inequalities, restrictions, and mechanical constraints tagged to the robotic systems and incorporate them in the controllers derived from LbCS, compared to the other motion and control schemes from literature [3, 23, 24]. For example, amongst others it is very difficult to capture the dynamical constraints (such as limitations on steering angle) of a system into the controllers, but LBCS has an in-built process of converting these into artificial obstacles and incorporating them into the nonlinear controllers.While many papers have considered motion planning and control ofType(nh,h) formation, this paper adds a new concept calledvirtual connectivity to formation control. This is where all the carrier robots are virtually connected to the lead robot along the trajectory to task completion. The concept inspired by the work by Consolini et al. in 2008 in [15] and Sharma et al. in 2012 in [25], is classified as the dϕ-strategy where d and ϕ are Euclidean measures of a lead-carrier robot scheme. Fixing the values of d enables us to maintain a fixed distance between the lead robot and each of its carriers. In addition, ϕ enables orientation change of the carriers which will optimize avoidance and the lane change and lane merge maneuvers.In this research, the team of robots is virtually connected, meaning that all agents are bounded mathematically as an automated and intelligent swarm in formation. While connected vehicles can normally be seen to facilitate the Vehicle-to-Vehicle (V2V) and the Vehicle-to-Infrastructure (V2I) communications, collectively defined as Vehicle-to-Everything (V2X) communications, on our roads more recently through wireless technologies such as cellular networks, this research takes a step towards possible applications of the connected vehicles through the dual-formation of an autonomous swarm in a obstacle-ridden environment, such as heavy-traffic highways with multiple lanes. In order to recognize this difference, we consider the swarm to be virtually connected and behave as an autonomous and intelligent robot system, controlled via the Lyapunov-based control scheme. Another advantage of the virtual connectivity is the fact that the lead-carrier team have the capability to satisfactorily complete diverse tasks which is not possible with merely cooperative agents. Typical examples of the connected mobile manipulators are the freight vehicles which are responsible for payload transfer, transportation of large objects, and transportation of multiple objects simultaneously on highways and roads, with safe and collision-free motion through vehicular internetworking and V2I communications. While the swarm intelligence ensures energy efficiency and reduced costs, the new dual-formation ensures multitasking, job precision and another solution to applications on roads such as convoying and payload transfer.It is the authors’ belief that the problem of maintaining adual-formation of a lead-carrier team with virtual connectively within an obstacle cluttered environment such as a heavy-traffic highway is treated for the first time within a framework of LbCS. While global rigidity of formations is not possible with nonholonomic constraints, we maintain local rigidity of the team in formation with reference to the end-effectors and also establish a globally rigid formation of every lead-carrier pair in the team. The overarching framework is a lead-carrier scheme to establish, maintain, and translate the whole team under dual-formation through centralized control laws.Finally, treatment of various categories of obstacles is included within the research problem. For connected and automated vehicle system on roads and highways, stationary obstacles can be treated as road pavements or lane boundaries which need to be avoided in order to contain and maintain individual or multiple vehicles in designated road lane(s). As an application, we consider a number of rigid-shaped objects needed to be dropped off at precise coordinates and bearings, en route a collision and obstacle-free trajectory. This is very useful in situations involving loading/offloading on docks, mines, and military bases and the lane changing and maneuverability on roads and highways, where precision is paramount.


## 1.1. Motivational Work

Researchers have continually designed and introduced numerous robotic systems that have a large array of possibilities for real world applications. One such system of increasing practical importance is based on swarm intelligence, which is a coordinated robot collective that is frequently sighted in airports, factories, wharfs, farms, and mines [1] and will play a huge role in the future as well. Swarms or flocks in nature are commonly dovetailed with formation types and unique patterns, seen, for example, in schools of fishes, flocks of birds, swarms of insects, and herds of animals. In addition, a wide spectrum of the “formation rigidity” has also appeared in literature using the nomenclature from [2, 3]; on one end there aresplit/rejoin maneuvers also known as minimally rigid formations [2, 3] which are required in applications such as reconnaissance, sampling, and surveillance, while on the other end there areglobally rigid formations which are required in applications that require cooperative payload transportation [4–6]. Then there arelocally rigid formations (required in convoying and demining [1, 7]) which are not infinitesimally rigid and allow for slight distortions temporarily. Adual-formation would be accomplishing two different formation types or patterns in parallel from a team in motion. However, it is noted that motion planning and control of a dual-formation are usually avoided due to the complexity of the algorithms and associated computer simulations and roll out to experimental designs.Formation control can be defined as controlling the configuration or state of a team of agents en route to a target, normally maintaining constant their relative locations, hence maintaining the formation type and pattern [8–11]. Indeed, scalable formations with increased heterogeneity are evidently deployed to satisfy stringent time, labor, and cost demands. The literature contains rich and diverse research on motion planning and control of such collectives, which are ideally suited and capable of performing wide-ranging tasks such as those mentioned above. These can take place in known or partially known environments; an environment may be harsh or hazardous or even inaccessible to humans [1]. Collectives provide increased performance, redundancy, flexibility, robustness, and multibehaviors which is difficult, and at times impossible, with single agents [12–14].A number of approaches such as leader-follower, virtual structure, nearest neighbors, social potentials, behavior-based, and formation-constrained functions have been utilized to address formation control [11]. While the lead-carrier approach is known for its poor disturbance rejection properties [15] and dependence placed upon a single agent, it is commonly utilized in literature because of its simplicity, scalability, and ability to contain an array of formations with richer specifications and complexities [3, 9, 11, 15–17]. This work deploys a new lead-carrier scheme to establish and control a team fixed in a dual-formation, for the first time.


## 1.2. Mechanical System

Of the many mechanical systems recorded in literature, mobile manipulators play a pivotal role in most industries that require a certain degree of automation and repetition. The literature takes into account the type of mobility of mobile manipulators with four possible configurations: (1)Type(h,h) where both platform and manipulator contain holonomic constraints, (2)Type(h,nh) where the platform is holonomic and manipulator is nonholonomic, (3)Type(nh,h) where the platform is nonholonomic but the manipulator is holonomic, and (4)Type(nh,nh) where both the platform and the manipulator contain nonholonomic constraints.In this paper we considerType(nh,h) which was introduced by Seraji in [18] in 1998 and classified as thenonholonomic mobile manipulators. Since then the literature has grown with research usingType(nh,h) mobile manipulators (see, e.g., [11, 19–21]). The challenges associated with motion planning and control of theType(nh,h) are amplified by the intimate coupling of the nonholonomic and holonomic constraints arising from the amalgamation of an articulated robotic arm and a wheeled platform.


## 1.3. Lyapunov-Based Control Scheme

The Lyapunov-based control scheme (LBCS), proposed by Sharma et al. in [3, 11], can be categorized under the artificial potential field method commonly deployed in the area of robotics research, especially applied to motion planning and control of various robotic systems [1, 3, 11]. We utilize the control scheme to derive centralized acceleration-based controllers for a dual-formation control of a team of lead-carrier 2-link mobile manipulators.The seminal idea behind LbCS is to design an appropriate Lyapunov function which acts astotal potentials. LbCS entails the construction of attractive and obstacle avoidance functions for the attraction to target and repulsion from various obstacles, respectively. While the attractive functions can be treated as attractive potential field functions, per se, the repulsive potential field functions are designed as ratios with the obstacle avoidance function in the denominator and a positivetuning parameter in the numerator. The sum of these potential functions is termed as thetotal potentials, a basis to design the controllers for the team. The governing principle behind the method is to attach attractive field to the target and repulsive field to each obstacle. The whole workspace is then inundated with positive and negative fields, with the direction of motion facilitated via the notion of steepest descent. For the vehicular systems, the gradient of the total potentials, referred to as the input force, determines the speed and the direction along which the vehicle moves.


## 1.4. Contributions

The Lyapunov-based control scheme utilized in this paper has been introduced in numerous applications for various robotic systems including ones tagged with holonomic or nonholonomic constraints [3, 22]. It is relatively easier to construct mathematical functions from limitations, inequalities, restrictions, and mechanical constraints tagged to the robotic systems and incorporate them in the controllers derived from LbCS, compared to the other motion and control schemes from literature [3, 23, 24]. For example, amongst others it is very difficult to capture the dynamical constraints (such as limitations on steering angle) of a system into the controllers, but LBCS has an in-built process of converting these into artificial obstacles and incorporating them into the nonlinear controllers.While many papers have considered motion planning and control ofType(nh,h) formation, this paper adds a new concept calledvirtual connectivity to formation control. This is where all the carrier robots are virtually connected to the lead robot along the trajectory to task completion. The concept inspired by the work by Consolini et al. in 2008 in [15] and Sharma et al. in 2012 in [25], is classified as the dϕ-strategy where d and ϕ are Euclidean measures of a lead-carrier robot scheme. Fixing the values of d enables us to maintain a fixed distance between the lead robot and each of its carriers. In addition, ϕ enables orientation change of the carriers which will optimize avoidance and the lane change and lane merge maneuvers.In this research, the team of robots is virtually connected, meaning that all agents are bounded mathematically as an automated and intelligent swarm in formation. While connected vehicles can normally be seen to facilitate the Vehicle-to-Vehicle (V2V) and the Vehicle-to-Infrastructure (V2I) communications, collectively defined as Vehicle-to-Everything (V2X) communications, on our roads more recently through wireless technologies such as cellular networks, this research takes a step towards possible applications of the connected vehicles through the dual-formation of an autonomous swarm in a obstacle-ridden environment, such as heavy-traffic highways with multiple lanes. In order to recognize this difference, we consider the swarm to be virtually connected and behave as an autonomous and intelligent robot system, controlled via the Lyapunov-based control scheme. Another advantage of the virtual connectivity is the fact that the lead-carrier team have the capability to satisfactorily complete diverse tasks which is not possible with merely cooperative agents. Typical examples of the connected mobile manipulators are the freight vehicles which are responsible for payload transfer, transportation of large objects, and transportation of multiple objects simultaneously on highways and roads, with safe and collision-free motion through vehicular internetworking and V2I communications. While the swarm intelligence ensures energy efficiency and reduced costs, the new dual-formation ensures multitasking, job precision and another solution to applications on roads such as convoying and payload transfer.It is the authors’ belief that the problem of maintaining adual-formation of a lead-carrier team with virtual connectively within an obstacle cluttered environment such as a heavy-traffic highway is treated for the first time within a framework of LbCS. While global rigidity of formations is not possible with nonholonomic constraints, we maintain local rigidity of the team in formation with reference to the end-effectors and also establish a globally rigid formation of every lead-carrier pair in the team. The overarching framework is a lead-carrier scheme to establish, maintain, and translate the whole team under dual-formation through centralized control laws.Finally, treatment of various categories of obstacles is included within the research problem. For connected and automated vehicle system on roads and highways, stationary obstacles can be treated as road pavements or lane boundaries which need to be avoided in order to contain and maintain individual or multiple vehicles in designated road lane(s). As an application, we consider a number of rigid-shaped objects needed to be dropped off at precise coordinates and bearings, en route a collision and obstacle-free trajectory. This is very useful in situations involving loading/offloading on docks, mines, and military bases and the lane changing and maneuverability on roads and highways, where precision is paramount.


## 2. System Modelling

LetAi,i∈{0,1,…,n} be the ith 2-link mobile manipulator (2MM) consisting of a car-like wheeled platform (front-wheel steered) with a 2-link planar arm mounted on the midfront axle of the wheeled platform. A0 and Al for l=1,…,n are the lead and carrier robots, respectively, of a team of 2MMs.Themth articulated body of the 2MM, m=1,2,3, is a disk with radius rim and is positioned at center (xim,yim) and it represents the set Aim={(z1,z2)∈R2:(z1-xim)2+(z2-yim)2≤rim}. Note that m=1 represents the wheeled platform while m=2,3 represent links 1 and 2, respectively (see Figure 1). Precisely, the ith 2MM is the set given by Ai={(z1,z2)∈R2:Ai1∪Ai2∪Ai3}.Figure 1
Schematic representation ofAi in the z1-z2 plane.The dimensions of each 2MMs are kept the same; that is,rim=rm for i∈{0,1,…,n}. The maximum speed υmax and maximum steering angle ϕmax of each mobile car-like platform are also kept the same.The planar workspace is a fixed, closed, and bounded rectangular region defined for someη1>2r1+r2+r3 and η2>2r1+r2+r3, as WS=z1,z2∈R2:0≤z1≤η1,0≤z2≤η2.With reference to Figure1, (xi1,yi1) gives the location of the center of the wheeled platform of Ai, while ψi is the steering angle with respect to the platform’s longitudinal axis. Now, θi1 gives the platform’s orientation with respect to the z1-axis, θi2 gives the orientation of Link 1 with respect to its platform, and θi3 gives the orientation of Link 2 with respect to Link 1. For simplicity, we let ψi=θi1.Note the presence ofclearance parametersϵ1,ϵ2,ϵ3>0 for safety of the wheeled platform and the gripper [1]. The position (xi,yi) of the end-effector of Ai with respect to its wheeled platform can be written as (1)xi=xi1+l12cos⁡θi1+l2cos⁡θi1+θi2+l3cos⁡θi1+θi2+θi3,yi=yi1+l12sin⁡θi1+l2sin⁡θi1+θi2+l3sin⁡θi1+θi2+θi3.One can comfortably show that the dynamic model of Ai for i∈{0,1,…,n} with respect to the end-effector is (2)x˙i=vicos⁡θi1-∑m=13∑k=m3lkωimsin⁡∑p=1kθip,y˙i=visin⁡θi1+∑m=13∑k=m3lkωimcos⁡∑p=1kθip,θ˙ij=ωij,v˙i=ui1,ω˙ij=uij+1,j=1,2,3.The reader is referred to [11] for a detailed derivation of the system of ODEs. We see that ui1 and ui2 are the instantaneous translational and rotational accelerations, respectively, of the wheeled platform, while ui3 and ui4 are the instantaneous angular accelerations, respectively, of the lower and upper links of Ai, the latter being relative accelerations since the torque and moment of inertia of the system are considered as constants. In addition, we assume no slippage (i.e., x˙sin⁡θ-y˙cos⁡θ=0) and pure rolling (i.e., x˙cos⁡θ+y˙sin⁡θ=υ) of the platform wheels. These nonintegrable constraints, denoted as the nonholonomic constraints, are captured in system (2).To ensure that the completeAi safely steers past an obstacle, we adopt the nomenclature of [1] to enclose each body of Ai by the smallest possible circle. Given the clearance parameters ϵ1,ϵ2,ϵ3>0 (see Figure 1), we enclose the wheeled platform by a protective circular region with radius r1=l1+2ϵ12+b1+2ϵ22/2, Link 1 with radius r2=l2/2, and Link 2 with radius r3=l3/2+ϵ3. This procedure provides the 2MMs with maximized free space.Furthermore, the positions of the articulated bodies of theith 2MM can be expressed completely in terms of the state variables xi, yi, θi1, θi2, and θi3. Hence, for the articulated bodies m=1,2,3 of Ai we ascertain (3)xim=xi-∑k=m3lk2m/kcos⁡∑p=1kθip,yim=yi-∑k=m3lk2m/ksin⁡∑p=1kθip,where m/k is a floor function. These position constraints are known as theholonomic constraints of the 2MM system [11].Note that for the target attraction and obstacle avoidance functions that will be defined in later sections, we consideri=0,1,…,n for Ai and m=1,2,3 for the mth articulated body of Ai.


## 3. Main Objective

The main objective is to design artificial potential functions (APFs) from LbCS described in [1, 3, 25, 26] and, accordingly, derive centralized acceleration-based controls such that the lead-carrier pairs in the team, represented by system (2), can transfer rigid rod-shaped loads, which approximate the desired orientations of the loads and the carriers. In addition, to ensure that the carriers move cohesively in a prescribed pattern and do not collide with each other, their end-effectors will also be fixed in a locally rigid formation. We work with a random number of carriers displaced from the lead robot. That is, we let set S=d1,d2,…,dn as the set of distances between the lead robot and the carriers.While each lead-carrier robot pair is maintained in a globally rigid formation (global rigidity) to carry rigid objects precisely, we also want the the whole team to be fixed in parallel within a locally rigid formation (local rigidity) to ensure an overall pattern for cohesiveness, safety, and specific task-requirements.The design of the nonlinear control laws is captured in Figure2, clearly illustrating the roles of the new control scheme and the dϕ-strategy.Figure 2
The design process of the nonlinear acceleration-based continuous control laws.


## 4. Globally Rigid Formation of Lead-Carrier Pairs

Aglobally rigid formation is a configuration in which distance and angle between the end-effectors of the lead-carrier pairs are strictly maintained en route to the destination. To ensure globally rigid formation of every lead-carrier pair and their cohesiveness, we propose a new strategy which we now classify as the dϕ-strategy.Thedϕ-strategy is inspired by the work carried out by Sharma et al. in 2014 in [25] and Consolini et al. in 2008 in [15]. In the latter, hierarchical formations were achieved for unicycles with input constraints on the admissible trajectories of the lead robot guiding the formation. The shape of the formation also changed according to the motion of the lead robot. The advantages of the dϕ-strategy over earlier leader-follower schemes from the authors [3, 26] are the following: (1) it contributes to the cohesion of the team; (2) the lead robot needs not be positioned in front of the pack; (3) there is a need for velocity consensus of the team; (4) there is no need to design separate attractive functions for the carriers, with the function established for the lead sufficing; and (5) rotation(s) of the formation is enabled, if needed, during avoidance maneuvers.Thedϕ-strategy establishes a virtual connection between the lead and each carrier of the team. This ensures that the lead-carrier distances required for the rigid formation are fixed and maintained at all times. Specifically, the strategy requires that the end-effector of the ith carrier robot be fixed via parameters di and ϕi with respect to the end-effector of the unconstrained leader (see Figure 3). While di is the length of the line from the end-effector of the leader to the end-effector of the ith carrier robot of the team, ϕi is the angle to the line with respect to the main frame (z1,z2) for i∈{1,…,n}.Figure 3
Schematic representation of the lead-carrier in thez1-z2 plane.Now, with reference to Figure3, we have (4)xi=x0+dicos⁡ϕi,yi=y0+disin⁡ϕi,which is true for ϕi=ϕi,π-ϕi,ϕi-π,2π-ϕi, considering all possible orientations in the workspace. The addition formula for cosine gives (5)2dicos⁡ϕi-π4=xi+yi-x0+y0,which differentiated as (6)ϕ˙i=1disin⁡ϕi-π/4v0sin⁡θ01+π4-∑m=13∑k=m3lkω0msin⁡∑p=1kθ0p-π4-visin⁡θi1+π4+∑m=13∑k=m3lkωimsin⁡∑p=1kθip-π4.Then the ODEs governing the dynamic model of n 2MMs, incorporating the dϕ-strategy, and hence ensuring virtual connectivity between all lead-carrier pairs can be rewritten as (7)x˙i=v0cos⁡θ01-∑m=13∑k=m3lkω0msin⁡∑p=1kθ0p-fisin⁡ϕi,y˙i=v0sin⁡θ01+∑m=13∑k=m3lkω0mcos⁡∑p=1kθ0p+ficos⁡ϕi,θ˙ij=ωij,v˙i=ui1,ω˙ij=uij+1,j=1,2,3,where (8)fi=1sin⁡ϕi-π/4v0sinθ01+π4-visinθi1+π4-∑m=13∑k=m3lkω0msin⁡∑p=1kθ0p-π4+∑m=13∑k=m3lkωimsin⁡∑p=1kθip-π4,for i∈0,1,…,n. We note that i=0 corresponds to the lead robot and if f(0)=0 system (7) basically collapses into system (2), which governs the motion of 2MMs not constrained by any connectivity. System (7) is a description of the instantaneous velocities and accelerations of the various bodies of Ai. We assume that the instantaneous accelerations ui1,ui2,ui3, and ui4 can move the end-effector of Ai, within the framework of virtual connectivity, to the final designation. Ultimately (ui1,ui2,ui3,ui4) for i=0,1,2,…,n are considered by the LbCS as the nonlinear acceleration controls of the 2MMs. Let the vector xi=xi,yi,θi1,θi2,θi3,vi,ωi1,ωi2,ωi3∈R9 refer to the position xi,yi of the end-effector of Ai, the orientations (θi1,θi2,θi3) of the various components of Ai, and the velocities vi,ωi1,ωi2,ωi3 of the various components of Ai at time t≥0. Let (9)qixi=qi1xi,…,qi5xi,0,0,0,0≔x˙i,y˙i,θ˙i1,θ˙i2,θ˙i3,0,0,0,0∈R9,and uit≔ui1t,ui2t,ui3t,ui4t∈R4. Then system (7) can be written compactly as (10)x˙i≔qixi+Biuit,where Bi is the 9×4 matrix (11)Bi=000000000000000000001000010000100001.Let x≔x0,x1,…,xn∈R9n+1 refer to the positions, orientations, and the velocities for n  2MMs, n∈N. Let q(x)≔q0x,q1x,…,qnx∈R9(n+1) and u(t)≔(u0(t),u1t,…,un(t))∈R4n+1. Then we have the following initial-value problem for n2MMs: (12)x˙=qx+But,xt0≕x0,t0≥0,where if 0 is the 9×4 matrix of all zero entries,(13)B=B00⋯00B1⋯0⋮⋮⋱⋮00⋯Bn.Now, assume that the final position of the end-effector of Ai is at the point (xi,yi)=(pi1,pi2) and final orientation at this point is (θi1,θi2,θi3)=(θi1f,θi2f,θi3f). Its final instantaneous velocity vector is (vi,ωi0,ωi1,ωi2)=(0,0,0,0). Then the points, (14)xi∗≔pi1,pi2,θi1f,θi2f,θi3f,0,0,0,0∈R9,are the components of the equilibrium point of system (12), with (15)x∗≔x0∗,x1∗,…,xn∗∈R9n+1.


## 5. Locally Rigid Formation of the Team

Alocally rigid formation is a configuration in which all the interrobot distances in a prescribed formation are maintained en route to the destination; however, temporary slight distortions are allowed. That is, the formation gives the end-effectors freedom to distort from their arrangement in the overall team pattern temporarily to be able to carry out a task effectively, such as collision avoidance of obstacles.Our work requires a coordinated transfer of loads by the lead-carrier team. Therefore, we need the team to be fixed in a locally rigid formation at all timest>0. The purpose of this is threefold: (1) it prevents the end-effectors from colliding or drifting off, therefore ensuring cohesion, (2) it maintains a prescribed formation which heralds order and purpose, and (3) it ensures that all lead-carrier pairs are contained in a neighborhood. We note that only the lead robot has the global information of the workspace, which includes the target configuration; thus, the carrier robots are guided by the lead robot. This implies the centralized nature of the problem.Inspired by the work of Li and Xiao [27], Schneider and Wildermuth [7], and Sharma et al. [1], we introduce a new design in order to establish, maintain, and translate a locally rigid formation during the motion. This design is an amalgamation of the following modules: (1) interrobot bounds, (2) target configuration, and (3) orientation consensus, which will be considered separately in the next sections. We will design the repulsive and attraction functions (which will be part of a Lyapunov function) to elucidate the importance and contribution of these modules to locally rigid formations.


### 5.1. Interrobot Bounds

#### 5.1.1. Minimum Interrobot Bound

To prevent any possible collisions between the end-effectors we shall adopt the following obstacle avoidance function from [25]: (16)MIijx=12xi-xj2+yi-yj2-Nij2,forj∈0,1,…,n,j≠i,where Nij2=4ϵ2 is the minimum Euclidean distance between end-effectors of Ai and Aj on R2.


#### 5.1.2. Maximum Interrobot Bound

The relative distance between the end-effectors of any two robots needs to be bounded. Accordingly, we design an obstacle avoidance function(17)MAijx=12Mij2-xi-xj2-yi-yj2,forj∈0,1,…,n,j≠i,where Mij2 is the maximum Euclidean distance between end-effectors of Ai and Aj on R2. These bounds are treated asartificial obstacles. To avoid these obstacles we design new repulsive potential field functions. The reader is referred to [25] for a detailed account of the relationship between bounds, artificial obstacles, and avoidance functions.To generate repulsive effects from (16) and (17) we design the correspondingrepulsive potential field function which in essence is a ratio that encodes the avoidance function in the denominator and a positivetuning parameter in the numerator. Manipulation of the tuning parameters associated with functions MAij and MIij provide an added degree of control and help maintain the locally rigid formation.Figure4 illustrates the total potentials generated from the attractive potentials produced by (19), and the repulsive potentials produced from α12/MA12 and γ12/MI12 to observe the maximum and minimum bounds between robots A1 and A2.Figure 4
Total potentials, withp1,p2=25,12.5,  r0=1.25, while α12=0.01, γ12=0.01, M12=5.0, and N12=3.0.Henceforth, for each obstacle (fixed, moving, or artificial), we will construct an appropriate avoidance function and, in accordance with the LBCS, design similar repulsive potential field functions to generate the collision and obstacle avoidance maneuvers. All-in-all the avoidance capability of the control scheme lies in the creation of repulsive potential functions that induce an increase or decrease in the instantaneous rate of change oftotal potentials.


### 5.2. Target Configuration

This work aims to imitate a real-life application wherein a number of rigid-shaped objects need to be dropped off simultaneously at precise coordinates and bearings, which is very useful in applications such as loading/offloading on docks or parking bays where precision is paramount. This translates to the control of the final configurations of the end-effectors.


#### 5.2.1. End-Effectors

First, we need to affix a target forA0, with respect to its end-effector, to reach after some time t>0. A possible target with center (p01,p02) and radius rt0 can be(18)T=z1,z2∈R2:z1-p012+z2-p022≤rt02.Second, the load is to be presented (deposited or dropped off) at the target with a precise orientation. Since we have the ends of the load held and carried by the end-effectors, there is a need to consider the final positional coordinates and orientation of each end-effector. However, with the introduction of thedϕ-strategy there is no need to establish separate attractive functions for the positional coordinates of the carriers, illustrating the centralized nature of the controllers.To establish an attraction to the final configuration ofA0 and the overall final bearing of the team, we consider attractive potential field functions(19)H0x=12x0-p012+y0+p022+∑j=23ρ0jθ0j-p0j+22+v02+∑k=13ω0k2,Hix=12∑j=23ρijθij-pij+22+vi2+∑k=13ωik2,for the lead and the ith carrier, respectively. The functions are positive for all x∈R9N+1. Note that θi1f,θi22,θi3f are the final orientations of the wheeled platform, Link 1 and Link 2, while ρi2,ρi3>0 are theangle-gain parameters, which have a value of 1 if a final orientation is warranted, or 0 as the default value [11]. Then in the LbCS, (19) will act as attractors by having the team move to the target configuration and ensure that system trajectories start and remain close to a stable equilibrium point of system (7).


#### 5.2.2. Mobile Platforms

In situations that reflect the objectives of this work, we observe that the mobile platforms of the carriers are parked at specific coordinates and bearings such as the parking bays. Since the end-effectors converge to the final target with the desired orientations of the links, the positional requirement of the mobile platforms is already captured in (19). Therefore, we need to consider only their final orientations and thus complete fixing the overall bearing of the team. For this we adopt theminimum distance technique (MDT) and the concept of ghost parking bays from [3, 26]. Ghost parking bays are constructed with precise coordinates in the neighborhood of the final configuration of each mobile platform, like caging with a single entrance.Utilizing MDT, we identify the closest point on each boundary line (treated as a line segment) of a ghost parking bay measured from theCoM of the wheeled platform of Ai. The underlying assumption of the technique is that avoidance of the closest point ensures avoidance of the entire line segment at every iteration t≥0. Now, the parametric representation of this kth line segment with initial coordinate (ak1,bk1) and final coordinate (ak2,bk2) where λik is the parameter is (20)cik=ak1+λikak2-ak1,dik=bk1+λikbk2-bk1.Minimizing the Euclidian distance from Ai to the kth line segment, we get (21)λik=xi1-ak1qk1+yi1-bk1qk2,where (22)qk1=ak2-ak1ak2-ak12+bk2-bk12,qk2=bk2-bk1ak2-ak12+bk2-bk12,while we utilize a saturation functionλik:R2→0,1 given as (23)λikxi,yi=0,ifλik<0λik,if0≤λik≤11,ifλik>1.We note that a specific ghost parking bay needs to be fixed for each 2MM; hence, a mobile platform will be surrounded by three boundary lines which have to be avoided. This means that the ith mobile platform will be avoiding (3i+1), (3i+2), and (3i+3) boundary lines. For avoidance, we consider a measurement LSik:R2→R+ of the distance between Ai and the closest point on kth boundary line (24)LSikx=12xi1-cik2+yi1-dik2-ri12,fork∈3i+1,3i+2,3i+3.To generate repulsive field around these ghost parking bays we again design new repulsive potential field functions, as discussed in the previous section.


### 5.3. Orientation Consensus

An important feature of locally rigid formation navigation of the robot team is its orientation consensus or, perhaps more frequently, being cited in literature ascommon heading. Potential functions which facilitate an orientation consensus are needed to be incorporated into the total potentials to provide the connected swarm or team with a common heading and to ensure that the mobile platforms are all orientated in the same direction. A failure of the orientation consensus not only unnecessarily upsets the locally rigid formation but also compromises the overall purpose of the transportation of transferring motion of the team. For example, the payload transportation or multiple-load transfer can be difficult or impossible if the mobile bases are not aligned and travelling on different headings with respect to the lead robot.In this work, the orientation consensus with reference to the mobile platform is established by the attractive function:(25)Rix=12θi1-1n∑j=1nθj12+wi1-1n∑j=1nwj12.This function is also responsive to the cohesion of the team where the lead’s orientation matches the average orientation of the team. The function stands well with the overall centralized architecture of the controls in this work, although the literature is rich with forms that contribute to distributed controls.


## 5.1. Interrobot Bounds

### 5.1.1. Minimum Interrobot Bound

To prevent any possible collisions between the end-effectors we shall adopt the following obstacle avoidance function from [25]: (16)MIijx=12xi-xj2+yi-yj2-Nij2,forj∈0,1,…,n,j≠i,where Nij2=4ϵ2 is the minimum Euclidean distance between end-effectors of Ai and Aj on R2.


### 5.1.2. Maximum Interrobot Bound

The relative distance between the end-effectors of any two robots needs to be bounded. Accordingly, we design an obstacle avoidance function(17)MAijx=12Mij2-xi-xj2-yi-yj2,forj∈0,1,…,n,j≠i,where Mij2 is the maximum Euclidean distance between end-effectors of Ai and Aj on R2. These bounds are treated asartificial obstacles. To avoid these obstacles we design new repulsive potential field functions. The reader is referred to [25] for a detailed account of the relationship between bounds, artificial obstacles, and avoidance functions.To generate repulsive effects from (16) and (17) we design the correspondingrepulsive potential field function which in essence is a ratio that encodes the avoidance function in the denominator and a positivetuning parameter in the numerator. Manipulation of the tuning parameters associated with functions MAij and MIij provide an added degree of control and help maintain the locally rigid formation.Figure4 illustrates the total potentials generated from the attractive potentials produced by (19), and the repulsive potentials produced from α12/MA12 and γ12/MI12 to observe the maximum and minimum bounds between robots A1 and A2.Figure 4
Total potentials, withp1,p2=25,12.5,  r0=1.25, while α12=0.01, γ12=0.01, M12=5.0, and N12=3.0.Henceforth, for each obstacle (fixed, moving, or artificial), we will construct an appropriate avoidance function and, in accordance with the LBCS, design similar repulsive potential field functions to generate the collision and obstacle avoidance maneuvers. All-in-all the avoidance capability of the control scheme lies in the creation of repulsive potential functions that induce an increase or decrease in the instantaneous rate of change oftotal potentials.


## 5.1.1. Minimum Interrobot Bound

To prevent any possible collisions between the end-effectors we shall adopt the following obstacle avoidance function from [25]: (16)MIijx=12xi-xj2+yi-yj2-Nij2,forj∈0,1,…,n,j≠i,where Nij2=4ϵ2 is the minimum Euclidean distance between end-effectors of Ai and Aj on R2.


## 5.1.2. Maximum Interrobot Bound

The relative distance between the end-effectors of any two robots needs to be bounded. Accordingly, we design an obstacle avoidance function(17)MAijx=12Mij2-xi-xj2-yi-yj2,forj∈0,1,…,n,j≠i,where Mij2 is the maximum Euclidean distance between end-effectors of Ai and Aj on R2. These bounds are treated asartificial obstacles. To avoid these obstacles we design new repulsive potential field functions. The reader is referred to [25] for a detailed account of the relationship between bounds, artificial obstacles, and avoidance functions.To generate repulsive effects from (16) and (17) we design the correspondingrepulsive potential field function which in essence is a ratio that encodes the avoidance function in the denominator and a positivetuning parameter in the numerator. Manipulation of the tuning parameters associated with functions MAij and MIij provide an added degree of control and help maintain the locally rigid formation.Figure4 illustrates the total potentials generated from the attractive potentials produced by (19), and the repulsive potentials produced from α12/MA12 and γ12/MI12 to observe the maximum and minimum bounds between robots A1 and A2.Figure 4
Total potentials, withp1,p2=25,12.5,  r0=1.25, while α12=0.01, γ12=0.01, M12=5.0, and N12=3.0.Henceforth, for each obstacle (fixed, moving, or artificial), we will construct an appropriate avoidance function and, in accordance with the LBCS, design similar repulsive potential field functions to generate the collision and obstacle avoidance maneuvers. All-in-all the avoidance capability of the control scheme lies in the creation of repulsive potential functions that induce an increase or decrease in the instantaneous rate of change oftotal potentials.


## 5.2. Target Configuration

This work aims to imitate a real-life application wherein a number of rigid-shaped objects need to be dropped off simultaneously at precise coordinates and bearings, which is very useful in applications such as loading/offloading on docks or parking bays where precision is paramount. This translates to the control of the final configurations of the end-effectors.


### 5.2.1. End-Effectors

First, we need to affix a target forA0, with respect to its end-effector, to reach after some time t>0. A possible target with center (p01,p02) and radius rt0 can be(18)T=z1,z2∈R2:z1-p012+z2-p022≤rt02.Second, the load is to be presented (deposited or dropped off) at the target with a precise orientation. Since we have the ends of the load held and carried by the end-effectors, there is a need to consider the final positional coordinates and orientation of each end-effector. However, with the introduction of thedϕ-strategy there is no need to establish separate attractive functions for the positional coordinates of the carriers, illustrating the centralized nature of the controllers.To establish an attraction to the final configuration ofA0 and the overall final bearing of the team, we consider attractive potential field functions(19)H0x=12x0-p012+y0+p022+∑j=23ρ0jθ0j-p0j+22+v02+∑k=13ω0k2,Hix=12∑j=23ρijθij-pij+22+vi2+∑k=13ωik2,for the lead and the ith carrier, respectively. The functions are positive for all x∈R9N+1. Note that θi1f,θi22,θi3f are the final orientations of the wheeled platform, Link 1 and Link 2, while ρi2,ρi3>0 are theangle-gain parameters, which have a value of 1 if a final orientation is warranted, or 0 as the default value [11]. Then in the LbCS, (19) will act as attractors by having the team move to the target configuration and ensure that system trajectories start and remain close to a stable equilibrium point of system (7).


### 5.2.2. Mobile Platforms

In situations that reflect the objectives of this work, we observe that the mobile platforms of the carriers are parked at specific coordinates and bearings such as the parking bays. Since the end-effectors converge to the final target with the desired orientations of the links, the positional requirement of the mobile platforms is already captured in (19). Therefore, we need to consider only their final orientations and thus complete fixing the overall bearing of the team. For this we adopt theminimum distance technique (MDT) and the concept of ghost parking bays from [3, 26]. Ghost parking bays are constructed with precise coordinates in the neighborhood of the final configuration of each mobile platform, like caging with a single entrance.Utilizing MDT, we identify the closest point on each boundary line (treated as a line segment) of a ghost parking bay measured from theCoM of the wheeled platform of Ai. The underlying assumption of the technique is that avoidance of the closest point ensures avoidance of the entire line segment at every iteration t≥0. Now, the parametric representation of this kth line segment with initial coordinate (ak1,bk1) and final coordinate (ak2,bk2) where λik is the parameter is (20)cik=ak1+λikak2-ak1,dik=bk1+λikbk2-bk1.Minimizing the Euclidian distance from Ai to the kth line segment, we get (21)λik=xi1-ak1qk1+yi1-bk1qk2,where (22)qk1=ak2-ak1ak2-ak12+bk2-bk12,qk2=bk2-bk1ak2-ak12+bk2-bk12,while we utilize a saturation functionλik:R2→0,1 given as (23)λikxi,yi=0,ifλik<0λik,if0≤λik≤11,ifλik>1.We note that a specific ghost parking bay needs to be fixed for each 2MM; hence, a mobile platform will be surrounded by three boundary lines which have to be avoided. This means that the ith mobile platform will be avoiding (3i+1), (3i+2), and (3i+3) boundary lines. For avoidance, we consider a measurement LSik:R2→R+ of the distance between Ai and the closest point on kth boundary line (24)LSikx=12xi1-cik2+yi1-dik2-ri12,fork∈3i+1,3i+2,3i+3.To generate repulsive field around these ghost parking bays we again design new repulsive potential field functions, as discussed in the previous section.


## 5.2.1. End-Effectors

First, we need to affix a target forA0, with respect to its end-effector, to reach after some time t>0. A possible target with center (p01,p02) and radius rt0 can be(18)T=z1,z2∈R2:z1-p012+z2-p022≤rt02.Second, the load is to be presented (deposited or dropped off) at the target with a precise orientation. Since we have the ends of the load held and carried by the end-effectors, there is a need to consider the final positional coordinates and orientation of each end-effector. However, with the introduction of thedϕ-strategy there is no need to establish separate attractive functions for the positional coordinates of the carriers, illustrating the centralized nature of the controllers.To establish an attraction to the final configuration ofA0 and the overall final bearing of the team, we consider attractive potential field functions(19)H0x=12x0-p012+y0+p022+∑j=23ρ0jθ0j-p0j+22+v02+∑k=13ω0k2,Hix=12∑j=23ρijθij-pij+22+vi2+∑k=13ωik2,for the lead and the ith carrier, respectively. The functions are positive for all x∈R9N+1. Note that θi1f,θi22,θi3f are the final orientations of the wheeled platform, Link 1 and Link 2, while ρi2,ρi3>0 are theangle-gain parameters, which have a value of 1 if a final orientation is warranted, or 0 as the default value [11]. Then in the LbCS, (19) will act as attractors by having the team move to the target configuration and ensure that system trajectories start and remain close to a stable equilibrium point of system (7).


## 5.2.2. Mobile Platforms

In situations that reflect the objectives of this work, we observe that the mobile platforms of the carriers are parked at specific coordinates and bearings such as the parking bays. Since the end-effectors converge to the final target with the desired orientations of the links, the positional requirement of the mobile platforms is already captured in (19). Therefore, we need to consider only their final orientations and thus complete fixing the overall bearing of the team. For this we adopt theminimum distance technique (MDT) and the concept of ghost parking bays from [3, 26]. Ghost parking bays are constructed with precise coordinates in the neighborhood of the final configuration of each mobile platform, like caging with a single entrance.Utilizing MDT, we identify the closest point on each boundary line (treated as a line segment) of a ghost parking bay measured from theCoM of the wheeled platform of Ai. The underlying assumption of the technique is that avoidance of the closest point ensures avoidance of the entire line segment at every iteration t≥0. Now, the parametric representation of this kth line segment with initial coordinate (ak1,bk1) and final coordinate (ak2,bk2) where λik is the parameter is (20)cik=ak1+λikak2-ak1,dik=bk1+λikbk2-bk1.Minimizing the Euclidian distance from Ai to the kth line segment, we get (21)λik=xi1-ak1qk1+yi1-bk1qk2,where (22)qk1=ak2-ak1ak2-ak12+bk2-bk12,qk2=bk2-bk1ak2-ak12+bk2-bk12,while we utilize a saturation functionλik:R2→0,1 given as (23)λikxi,yi=0,ifλik<0λik,if0≤λik≤11,ifλik>1.We note that a specific ghost parking bay needs to be fixed for each 2MM; hence, a mobile platform will be surrounded by three boundary lines which have to be avoided. This means that the ith mobile platform will be avoiding (3i+1), (3i+2), and (3i+3) boundary lines. For avoidance, we consider a measurement LSik:R2→R+ of the distance between Ai and the closest point on kth boundary line (24)LSikx=12xi1-cik2+yi1-dik2-ri12,fork∈3i+1,3i+2,3i+3.To generate repulsive field around these ghost parking bays we again design new repulsive potential field functions, as discussed in the previous section.


## 5.3. Orientation Consensus

An important feature of locally rigid formation navigation of the robot team is its orientation consensus or, perhaps more frequently, being cited in literature ascommon heading. Potential functions which facilitate an orientation consensus are needed to be incorporated into the total potentials to provide the connected swarm or team with a common heading and to ensure that the mobile platforms are all orientated in the same direction. A failure of the orientation consensus not only unnecessarily upsets the locally rigid formation but also compromises the overall purpose of the transportation of transferring motion of the team. For example, the payload transportation or multiple-load transfer can be difficult or impossible if the mobile bases are not aligned and travelling on different headings with respect to the lead robot.In this work, the orientation consensus with reference to the mobile platform is established by the attractive function:(25)Rix=12θi1-1n∑j=1nθj12+wi1-1n∑j=1nwj12.This function is also responsive to the cohesion of the team where the lead’s orientation matches the average orientation of the team. The function stands well with the overall centralized architecture of the controls in this work, although the literature is rich with forms that contribute to distributed controls.


## 6. Integrated Subtasks

We further include in this work a number of kinodynamic constraints from the workspace or ones tagged to the 2MMs.


### 6.1. Avoidance of Stationary Obstacles

Let us fixq stationary obstacles within the boundaries of the workspace. We assume that the lth stationary obstacle, for l=1,2,…,q, is circular with center given as ol1,ol2 and radius radl and defined as Ol≔{(z1,z2)∈R2:(z1-ol1)2+(z2-ol2)2≤radl2}. Note that it is only for simplicity of analysis and illustration of the Lyapunov-based methodology that we have chosen circular obstacles. However, any convex polygonal obstacle can be considered in our methodology because we can apply the MDT (minimum distance technique) that we utilized in Section 5.2.2 to ensure avoidance between the end-effector and the nearest point of a line segment.For the avoidance of the stationary circular obstacles we construct separate avoidance functions for eachm body of Ai: (26)FOimlx=12xim-ol12+yim-ol22-rm+radl2,forl=1,…,q.For the vehicle system on roads and highways, these stationary obstacles can be treated as road pavements or lane boundaries which need to be avoided in order to contain and maintain individual or multiple vehicles in designated road lane(s). The road infrastructure is assumed to be well equipped with sensors and wireless technologies to ensure adequate and timely Vehicle-to-Infrastructure (V2I) communication for the real-life applications of the automated and intelligent vehicle system to work. The assumption is applicable keeping in mind that the current proposed engineering solutions (to the problem of allowing autonomous vehicles on public roads) include the development of special lanes on roads purely for self-driving vehicles imbued with the special sensors and wireless technologies.


### 6.2. Avoidance of Moving Obstacles

Each solid body of an articulated 2MM has to be treated as a moving obstacle for all the other 2MMs in WS. Avoidance of the end-effectors is already captured in the recipe to generate and maintain the locally rigid formation. Therefore, for the platform ofAi to avoid the platform of Aj, we shall use an avoidance function: (27)MOijx=12xi1-xj12+yi1-yj12-2r12,forj=0,1,…,n,j≠i.Mechanical singularities and bounds on velocities are treated as dynamic constraints. In practice, bending angles of the links are limited due to the mechanical singularities, while the velocities of the links and the wheeled platform are restricted due to safety reasons [1]. In accordance with the LbCS, each dynamic constraint is treated as anartificial obstacle and appropriate obstacle avoidance function designed for the avoidance.


### 6.3. Mechanical Singularities

Singular configurations arise when we have the following.(i)
θi3=0, θi3=π, or θi3=-π. Subsequently, the condition placed on θi3 is 0<θi3<π for θi3∈-π,0∪0,π, which implies that Link 2 can neither be fully stretched nor be folded back.(ii)
The angle between Link 1 and the platform is bounded by-π/2<θi2<π/2. Simply worded, Link 1 of the ith 2MM can only freely rotate within -π/2,π/2.(iii)
Due to the inclusion of thedϕ-strategy, singularities arise when ϕi-π/4∈-π,0,π. To avoid this we should include 0<ϕi-π/4<π, for i=0,1,…,n.For avoidance of these singularities, the following obstacle avoidance functions will be included: (28a)Si1x=θi3,(28b)Si2x=π-θi3,(28c)Si4x=ϕi-π4,(28d)Si5x=π-ϕi-π4,(28e)Si3x=12π2-θi2π2+θi2.These positive functions would guarantee a strict observation of the mechanical singularities of the 2MMs when encoded into specific repulsive potential field function.


### 6.4. Modulus Bound on Velocities

From a practical viewpoint, the translational and rotational velocities of the 2MMs are limited, so we include the following constraints:(i)
vi<vmax, where vmax is themaximal achievable speed.(ii)
ωi1<vmax/|ρmin|, where ρmin=l0/tan⁡ϕmax. This condition arises from the boundedness of the steering angle, ϕi. That is ψi≤ψmax, where ψmax ismaximal steering angle.(iii)
ωi2<ω2max and ωi3<ω3max, where ω2max, ω3max are themaximal rotational velocities of Link 1 and Link 2, respectively.We construct avoidance functions so that Ai can successfully avoid the artificial obstacles created from the constraints above: (29a)Ui1x=12vmax2-vi2,(29b)Ui2x=12vmax2ρmin2-ωi12,(29c)Ui3x=12ω2max2-ωi22,(29d)Ui4x=12ω3max2-ωi32,for i=0,1,…,n. These positive functions guarantee the adherence to limitations when encoded appropriately into the repulsive potential field functions.


## 6.1. Avoidance of Stationary Obstacles

Let us fixq stationary obstacles within the boundaries of the workspace. We assume that the lth stationary obstacle, for l=1,2,…,q, is circular with center given as ol1,ol2 and radius radl and defined as Ol≔{(z1,z2)∈R2:(z1-ol1)2+(z2-ol2)2≤radl2}. Note that it is only for simplicity of analysis and illustration of the Lyapunov-based methodology that we have chosen circular obstacles. However, any convex polygonal obstacle can be considered in our methodology because we can apply the MDT (minimum distance technique) that we utilized in Section 5.2.2 to ensure avoidance between the end-effector and the nearest point of a line segment.For the avoidance of the stationary circular obstacles we construct separate avoidance functions for eachm body of Ai: (26)FOimlx=12xim-ol12+yim-ol22-rm+radl2,forl=1,…,q.For the vehicle system on roads and highways, these stationary obstacles can be treated as road pavements or lane boundaries which need to be avoided in order to contain and maintain individual or multiple vehicles in designated road lane(s). The road infrastructure is assumed to be well equipped with sensors and wireless technologies to ensure adequate and timely Vehicle-to-Infrastructure (V2I) communication for the real-life applications of the automated and intelligent vehicle system to work. The assumption is applicable keeping in mind that the current proposed engineering solutions (to the problem of allowing autonomous vehicles on public roads) include the development of special lanes on roads purely for self-driving vehicles imbued with the special sensors and wireless technologies.


## 6.2. Avoidance of Moving Obstacles

Each solid body of an articulated 2MM has to be treated as a moving obstacle for all the other 2MMs in WS. Avoidance of the end-effectors is already captured in the recipe to generate and maintain the locally rigid formation. Therefore, for the platform ofAi to avoid the platform of Aj, we shall use an avoidance function: (27)MOijx=12xi1-xj12+yi1-yj12-2r12,forj=0,1,…,n,j≠i.Mechanical singularities and bounds on velocities are treated as dynamic constraints. In practice, bending angles of the links are limited due to the mechanical singularities, while the velocities of the links and the wheeled platform are restricted due to safety reasons [1]. In accordance with the LbCS, each dynamic constraint is treated as anartificial obstacle and appropriate obstacle avoidance function designed for the avoidance.


## 6.3. Mechanical Singularities

Singular configurations arise when we have the following.(i)
θi3=0, θi3=π, or θi3=-π. Subsequently, the condition placed on θi3 is 0<θi3<π for θi3∈-π,0∪0,π, which implies that Link 2 can neither be fully stretched nor be folded back.(ii)
The angle between Link 1 and the platform is bounded by-π/2<θi2<π/2. Simply worded, Link 1 of the ith 2MM can only freely rotate within -π/2,π/2.(iii)
Due to the inclusion of thedϕ-strategy, singularities arise when ϕi-π/4∈-π,0,π. To avoid this we should include 0<ϕi-π/4<π, for i=0,1,…,n.For avoidance of these singularities, the following obstacle avoidance functions will be included: (28a)Si1x=θi3,(28b)Si2x=π-θi3,(28c)Si4x=ϕi-π4,(28d)Si5x=π-ϕi-π4,(28e)Si3x=12π2-θi2π2+θi2.These positive functions would guarantee a strict observation of the mechanical singularities of the 2MMs when encoded into specific repulsive potential field function.


## 6.4. Modulus Bound on Velocities

From a practical viewpoint, the translational and rotational velocities of the 2MMs are limited, so we include the following constraints:(i)
vi<vmax, where vmax is themaximal achievable speed.(ii)
ωi1<vmax/|ρmin|, where ρmin=l0/tan⁡ϕmax. This condition arises from the boundedness of the steering angle, ϕi. That is ψi≤ψmax, where ψmax ismaximal steering angle.(iii)
ωi2<ω2max and ωi3<ω3max, where ω2max, ω3max are themaximal rotational velocities of Link 1 and Link 2, respectively.We construct avoidance functions so that Ai can successfully avoid the artificial obstacles created from the constraints above: (29a)Ui1x=12vmax2-vi2,(29b)Ui2x=12vmax2ρmin2-ωi12,(29c)Ui3x=12ω2max2-ωi22,(29d)Ui4x=12ω3max2-ωi32,for i=0,1,…,n. These positive functions guarantee the adherence to limitations when encoded appropriately into the repulsive potential field functions.


## 7. Controller Design

We now define a Lyapunov function and subsequently extract from it the nonlinear control laws for system (7). However, to guarantee that the total potentials vanish precisely at the equilibrium state, we design an auxiliary function that would be multiplied to the repulsive potential field functions.


### 7.1. Auxiliary Function

(30)
F
i
x
=
1
2
x
i
-
p
i
1
2
+
y
i
-
p
i
2
2
+
∑
j
=
1
3
ρ
i
j
θ
i
j
-
p
i
j
+
2
2
.


### 7.2. Lyapunov Function

Combining the attractive and repulsive potential field functions and introducing tuning parameters,αij>0,ξip>0,γiml>0,ζik>0,βir>0 and φij>0 for i,j,l,m,p∈N, we consider a Lyapunov function for system (12) (suppressing x): (31)L=∑i=0nHi+Ri+Fi∑j=0j≠inαijMAij+φijMIij+ψijMOij+∑k=3i+13i+3ζikLSik+∑r=14βirUir+∑p=13ξipSip+∑m=13∑l=1qγimlFOiml+∑i=1n∑p=45FiξipSip.


### 7.3. Nonlinear Acceleration Controllers

We utilize LbCS [28] to derive the acceleration-based controllers. It is easy to see that the Lyapunov function in (31) is continuous and positive and has continuous first partial derivatives on the domain: (32)D=x∈R9N+1:MIijx>0,MAijx>0,MOijx>0,i,j∈0,1,…,n,j≠i;LSikx>0,k∈3i+1,3i+2,3i+3;FOimlx>0,m=1,2,3,l∈1,2,…,q;Sipx>0,p∈1,2,…,5;Uirx>0,r∈1,2,3,4.Moreover, L(x) vanishes at the equilibrium point x∗. Let δi1>0, δi2>0, δi3>0, and δi4>0. The time derivative of L, along a solution of system (12), is (33)L˙5x=-∑i=0nδi1vi2+δi2ωi12+δi3ωi22+δi4ωi32,provided we set the control laws as(34)ui1=-δi1vi+fi1+fi3+fi5+fi7cos⁡θi1+fi2+fi4+fi6+fi8sin⁡θi1gi4,ui2=-δi2ωi1-fi1+12fi3+fi5+fi7l1sin⁡θi1+fi2+12fi4+fi6+fi8l1cos⁡θi1-fi1+12fi5+fi7l2sin⁡θiQ+fi2+12fi6+fi8l2cos⁡θiQ-fi1+12fi7l3sin⁡θiT+fi2+12fi8l3cos⁡θiT+gi1gi5-1,ui3=-δi3ωi2-fi1+12fi5+fi7l2sin⁡θiQ+fi2+12fi6+fi8l2cos⁡θiQ-fi1+12fi7l3sin⁡θiT+fi2+12fi8l3cos⁡θiT+gi2gi6-1,ui4=-δi4ωi3-fi1+1/2fi7l3sin⁡θiT+fi2+1/2fi8l3cos⁡θiT+gi3gi7,where θiT=θi1+θi2, θiQ=θi1+θi2+θi3, δij>0, for i∈{0,1,2,…,n} and j∈{1,2,3,4} are constants classified asconvergence parameters and the functions fi1,fi2,…,fi8 and gi1,gi2,…,gi7 are defined as (35)f01=1+∑p=15ξ0pS0p+∑m=13∑l=1qγ0mlFO0ml+∑j=1nφ0jMI0j+α0jMA0j+ψ0jMO0j+∑k=13ζ0kLS0k+∑r=14β0rU0rx0-p01-∑i=1nFiϕi-π/42diϕi-π/4sin⁡ϕi-π/4ξi4Si42-ξi5Si52-∑j=1nψ0jFiMO0j2+ψj0FjMOj02x0-xj,fi1=∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUirxi-pi1+Fiϕi-π/42diϕi-π/4sin⁡ϕi-π/4ξi4Si42-ξi5Si52-∑j=0j≠inψijFiMOij2+ψjiFjMOji2xi-xj,f02=1+∑p=15ξ0pS0p+∑m=13∑l=1qγ0mlFO0ml+∑j=1nφ0jMI0j+α0jMA0j+ψ0jMO0j+∑k=13ζ0kLS0k+∑r=14β0rU0ry0-p02-∑i=1nFiϕi-π/42diϕi-π/4sin⁡ϕi-π/4ξi4Si42-ξi5Si52-∑j=1nψ0jFiMO0j2+ψj0FjMOj02y0-yj,fi2=∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUiryi-pi2+Fiϕi-π/42diϕi-π/4sin⁡ϕi-π/4ξi4Si42-ξi5Si52-∑j=0j≠inψijFiMOij2+ψjiFjMOji2yi-yj,fi3=-∑l=1qγi1lFiFOi1l2xi1-ol1-∑j=0j≠inψijFiMOij2+ψjiFjMOji2xi1-xj1-∑k=13ψikFiLSik21-ak2-ak1qk1xi1-cik-bk2-bk1qk1yi1-dik,fi4=-∑l=1qγi1lFiFOi1l2yi1-ol2-∑j=0j≠inψijFiMOij2+ψjiFjMOji2yi1-yj1-∑k=13ψikFiLSik21-bk2-bk1qk2yi1-dik-ak2-ak1qk2xi1-cik,fi5=-∑l=1qγi2lFiFOi2l2xi2-ol1,fi6=-∑l=1qγi2lFiFOi2l2yi2-ol2,fi7=-∑l=1qγi3lFiFOi3l2xi3-ol1,fi8=-∑l=1qγi3lFOi3l2yi3-ol2,gi1=θi1-1n∑j=1nθj1+∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUirρi1θi1-pi3,gi2=1+∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUirρi2θi2-pi4+ξi3FiSi32θi2,gi3=1+∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUirρi3θi3-pi5+Fiξi1Si12-ξi2Si22θi3θi3,gi4=1+βi1FiUi12,gi5=1+βi2FiUi22+ωi1-1n∑j=1nωj1,gi6=1+βi3FiUi32,gi7=1+βi4FiUi42.


### 7.4. Proof of Stability of Equilibrium Point

We note that LbCS produces feedback controllers, which depend explicitly on the state variables,x=x(t), and hence implicitly on time, t. That is, ui1(t)=ui1(x(t)), ui2(t)=ui2(x(t)), ui3(t)=ui3(x(t)), and ui4(t)=ui4(x(t)). Thus, we can let h(x)≔q(x)+Bu(x).Theorem 1.
The equilibrium pointx∗ of system (12) is stable provided ui1,ui2,ui3,ui4  for, i=1,…,n, are defined as in (34).Proof.
The higher-order partial derivatives ofui1, ui2, ui3, and ui4 are continuous on D for the simple fact that functions that appear in the denominator of these functions are in D and will also appear in the higher-order partial derivatives of the controllers. Since the other ODEs in (12) also have higher-order partial derivatives in x, with each continuous over D, we have that h∈C1D,R9N+1; that is, h is locally Lipschitz on D. This implies there are unique solutions of system (12) in D on some finite time interval 0,α,α>0. To prove their stability, we see that Lx>0 for all x∈D/x∗, Lx∗=0, L˙5x≤0 for all x∈D, and L˙(5)x∗=0. Hence, L∈C1D,R+, where R+=0,+∞, and Lx is a Lyapunov function for system (12). Thus, not only does the conclusion of Theorem 1 readily follows from the Direct Method of Lyapunov, but also that the solutions of system (12) globally exist and are unique and bounded on 0,∞ in D.Corollary 2.
Every solutionx∈D of system (12) converges to the largest invariant set contained in S0≔x∈D:L˙(x)=0.Proof.
By Theorem1, all solutions of system (12) in D are bounded. Hence the convergence to the largest invariance set in S0 is guaranteed by LaSalle’s invariance principle. The fact that x(t)→S0 as t→∞ shows that a trajectory of system (12), with an appropriate initial condition, could approach a neighborhood of x∗∈S0.
This does not contradict with Brockett’s result on nonholonomic systems because it is clear that the largest invariant set inS0 does not contain only x∗. In other words, our result guarantees only the stability for this type of system. There are methods that yield controllers guaranteeing asymptotic stability for general systems, some recent ones of which are referenced in [29]; however, these are no continuous time-invariant controls. To the authors’ knowledge, this paper is a first attempt to construct bounded yet continuous time-invariant controllers for the specific case of a multiple system represented in (12) via LbCS and the LaSalle’s invariance principle. Even though our result guarantees only stability, the approach meets our main objective which is to derive centralized acceleration controls such that the lead-carrier pairs in the team, represented by system (2), can transfer rigid rod-shaped loads to a small neighborhood of the final destination and can approximate the desired orientations of the loads and the carriers fixed in a dual-formation.


### 7.5. Proof of Collision Avoidance

The fact that solutions of system (12) globally exist and are unique and bounded in D means if x0∈D, then xt∈D for all t≥0. This, in turn, implies that the avoidance of every type of obstacle, physical or artificial, is guaranteed via the Lyapunov function L. To prove that D is a positively invariant set, we simply invoke the existence, uniqueness, and continuity of the solutions of (12) in a standard argument expounded in Khalil [30], page 653.Corollary 3.
The setD(V) is positively invariant.Proof.
Theorem1 and Corollary 2 guarantee the global existence, uniqueness, and boundedness of the solutions on 0,∞ in D. This allows us to let, say, χt;y∈D be the solution of (12) that passes through a point y∈D at t=0; that is, χ0;y=y∈D. In other words, for a solution xt∈D, there must be a sequence {tk} with tk→∞ such that xtk→y as k→∞. One then has that xtk=χtk;x0, where x0 is the initial state of xt at t=0. By the uniqueness of solutions,(36)χt+tk;x0=χt;χt;x0=χt;xtk,where, for sufficiently large k, t+tk>0. By the continuity of solutions,(37)limt→∞⁡χt+tk;x0=limt→∞⁡χt;xtk=χt;y,which shows that (38)χ0;y∈DV⟹χt;y∈D∀t≥0,∀y∈DV.


## 7.1. Auxiliary Function

(30)
F
i
x
=
1
2
x
i
-
p
i
1
2
+
y
i
-
p
i
2
2
+
∑
j
=
1
3
ρ
i
j
θ
i
j
-
p
i
j
+
2
2
.


## 7.2. Lyapunov Function

Combining the attractive and repulsive potential field functions and introducing tuning parameters,αij>0,ξip>0,γiml>0,ζik>0,βir>0 and φij>0 for i,j,l,m,p∈N, we consider a Lyapunov function for system (12) (suppressing x): (31)L=∑i=0nHi+Ri+Fi∑j=0j≠inαijMAij+φijMIij+ψijMOij+∑k=3i+13i+3ζikLSik+∑r=14βirUir+∑p=13ξipSip+∑m=13∑l=1qγimlFOiml+∑i=1n∑p=45FiξipSip.


## 7.3. Nonlinear Acceleration Controllers

We utilize LbCS [28] to derive the acceleration-based controllers. It is easy to see that the Lyapunov function in (31) is continuous and positive and has continuous first partial derivatives on the domain: (32)D=x∈R9N+1:MIijx>0,MAijx>0,MOijx>0,i,j∈0,1,…,n,j≠i;LSikx>0,k∈3i+1,3i+2,3i+3;FOimlx>0,m=1,2,3,l∈1,2,…,q;Sipx>0,p∈1,2,…,5;Uirx>0,r∈1,2,3,4.Moreover, L(x) vanishes at the equilibrium point x∗. Let δi1>0, δi2>0, δi3>0, and δi4>0. The time derivative of L, along a solution of system (12), is (33)L˙5x=-∑i=0nδi1vi2+δi2ωi12+δi3ωi22+δi4ωi32,provided we set the control laws as(34)ui1=-δi1vi+fi1+fi3+fi5+fi7cos⁡θi1+fi2+fi4+fi6+fi8sin⁡θi1gi4,ui2=-δi2ωi1-fi1+12fi3+fi5+fi7l1sin⁡θi1+fi2+12fi4+fi6+fi8l1cos⁡θi1-fi1+12fi5+fi7l2sin⁡θiQ+fi2+12fi6+fi8l2cos⁡θiQ-fi1+12fi7l3sin⁡θiT+fi2+12fi8l3cos⁡θiT+gi1gi5-1,ui3=-δi3ωi2-fi1+12fi5+fi7l2sin⁡θiQ+fi2+12fi6+fi8l2cos⁡θiQ-fi1+12fi7l3sin⁡θiT+fi2+12fi8l3cos⁡θiT+gi2gi6-1,ui4=-δi4ωi3-fi1+1/2fi7l3sin⁡θiT+fi2+1/2fi8l3cos⁡θiT+gi3gi7,where θiT=θi1+θi2, θiQ=θi1+θi2+θi3, δij>0, for i∈{0,1,2,…,n} and j∈{1,2,3,4} are constants classified asconvergence parameters and the functions fi1,fi2,…,fi8 and gi1,gi2,…,gi7 are defined as (35)f01=1+∑p=15ξ0pS0p+∑m=13∑l=1qγ0mlFO0ml+∑j=1nφ0jMI0j+α0jMA0j+ψ0jMO0j+∑k=13ζ0kLS0k+∑r=14β0rU0rx0-p01-∑i=1nFiϕi-π/42diϕi-π/4sin⁡ϕi-π/4ξi4Si42-ξi5Si52-∑j=1nψ0jFiMO0j2+ψj0FjMOj02x0-xj,fi1=∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUirxi-pi1+Fiϕi-π/42diϕi-π/4sin⁡ϕi-π/4ξi4Si42-ξi5Si52-∑j=0j≠inψijFiMOij2+ψjiFjMOji2xi-xj,f02=1+∑p=15ξ0pS0p+∑m=13∑l=1qγ0mlFO0ml+∑j=1nφ0jMI0j+α0jMA0j+ψ0jMO0j+∑k=13ζ0kLS0k+∑r=14β0rU0ry0-p02-∑i=1nFiϕi-π/42diϕi-π/4sin⁡ϕi-π/4ξi4Si42-ξi5Si52-∑j=1nψ0jFiMO0j2+ψj0FjMOj02y0-yj,fi2=∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUiryi-pi2+Fiϕi-π/42diϕi-π/4sin⁡ϕi-π/4ξi4Si42-ξi5Si52-∑j=0j≠inψijFiMOij2+ψjiFjMOji2yi-yj,fi3=-∑l=1qγi1lFiFOi1l2xi1-ol1-∑j=0j≠inψijFiMOij2+ψjiFjMOji2xi1-xj1-∑k=13ψikFiLSik21-ak2-ak1qk1xi1-cik-bk2-bk1qk1yi1-dik,fi4=-∑l=1qγi1lFiFOi1l2yi1-ol2-∑j=0j≠inψijFiMOij2+ψjiFjMOji2yi1-yj1-∑k=13ψikFiLSik21-bk2-bk1qk2yi1-dik-ak2-ak1qk2xi1-cik,fi5=-∑l=1qγi2lFiFOi2l2xi2-ol1,fi6=-∑l=1qγi2lFiFOi2l2yi2-ol2,fi7=-∑l=1qγi3lFiFOi3l2xi3-ol1,fi8=-∑l=1qγi3lFOi3l2yi3-ol2,gi1=θi1-1n∑j=1nθj1+∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUirρi1θi1-pi3,gi2=1+∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUirρi2θi2-pi4+ξi3FiSi32θi2,gi3=1+∑p=15ξipSip+∑m=13∑l=1qγimlFOiml+∑j=0j≠inφijMIij+αijMAij+ψijMOij+∑k=13ζikLSik+∑r=14βirUirρi3θi3-pi5+Fiξi1Si12-ξi2Si22θi3θi3,gi4=1+βi1FiUi12,gi5=1+βi2FiUi22+ωi1-1n∑j=1nωj1,gi6=1+βi3FiUi32,gi7=1+βi4FiUi42.


## 7.4. Proof of Stability of Equilibrium Point

We note that LbCS produces feedback controllers, which depend explicitly on the state variables,x=x(t), and hence implicitly on time, t. That is, ui1(t)=ui1(x(t)), ui2(t)=ui2(x(t)), ui3(t)=ui3(x(t)), and ui4(t)=ui4(x(t)). Thus, we can let h(x)≔q(x)+Bu(x).Theorem 1.
The equilibrium pointx∗ of system (12) is stable provided ui1,ui2,ui3,ui4  for, i=1,…,n, are defined as in (34).Proof.
The higher-order partial derivatives ofui1, ui2, ui3, and ui4 are continuous on D for the simple fact that functions that appear in the denominator of these functions are in D and will also appear in the higher-order partial derivatives of the controllers. Since the other ODEs in (12) also have higher-order partial derivatives in x, with each continuous over D, we have that h∈C1D,R9N+1; that is, h is locally Lipschitz on D. This implies there are unique solutions of system (12) in D on some finite time interval 0,α,α>0. To prove their stability, we see that Lx>0 for all x∈D/x∗, Lx∗=0, L˙5x≤0 for all x∈D, and L˙(5)x∗=0. Hence, L∈C1D,R+, where R+=0,+∞, and Lx is a Lyapunov function for system (12). Thus, not only does the conclusion of Theorem 1 readily follows from the Direct Method of Lyapunov, but also that the solutions of system (12) globally exist and are unique and bounded on 0,∞ in D.Corollary 2.
Every solutionx∈D of system (12) converges to the largest invariant set contained in S0≔x∈D:L˙(x)=0.Proof.
By Theorem1, all solutions of system (12) in D are bounded. Hence the convergence to the largest invariance set in S0 is guaranteed by LaSalle’s invariance principle. The fact that x(t)→S0 as t→∞ shows that a trajectory of system (12), with an appropriate initial condition, could approach a neighborhood of x∗∈S0.
This does not contradict with Brockett’s result on nonholonomic systems because it is clear that the largest invariant set inS0 does not contain only x∗. In other words, our result guarantees only the stability for this type of system. There are methods that yield controllers guaranteeing asymptotic stability for general systems, some recent ones of which are referenced in [29]; however, these are no continuous time-invariant controls. To the authors’ knowledge, this paper is a first attempt to construct bounded yet continuous time-invariant controllers for the specific case of a multiple system represented in (12) via LbCS and the LaSalle’s invariance principle. Even though our result guarantees only stability, the approach meets our main objective which is to derive centralized acceleration controls such that the lead-carrier pairs in the team, represented by system (2), can transfer rigid rod-shaped loads to a small neighborhood of the final destination and can approximate the desired orientations of the loads and the carriers fixed in a dual-formation.


## 7.5. Proof of Collision Avoidance

The fact that solutions of system (12) globally exist and are unique and bounded in D means if x0∈D, then xt∈D for all t≥0. This, in turn, implies that the avoidance of every type of obstacle, physical or artificial, is guaranteed via the Lyapunov function L. To prove that D is a positively invariant set, we simply invoke the existence, uniqueness, and continuity of the solutions of (12) in a standard argument expounded in Khalil [30], page 653.Corollary 3.
The setD(V) is positively invariant.Proof.
Theorem1 and Corollary 2 guarantee the global existence, uniqueness, and boundedness of the solutions on 0,∞ in D. This allows us to let, say, χt;y∈D be the solution of (12) that passes through a point y∈D at t=0; that is, χ0;y=y∈D. In other words, for a solution xt∈D, there must be a sequence {tk} with tk→∞ such that xtk→y as k→∞. One then has that xtk=χtk;x0, where x0 is the initial state of xt at t=0. By the uniqueness of solutions,(36)χt+tk;x0=χt;χt;x0=χt;xtk,where, for sufficiently large k, t+tk>0. By the continuity of solutions,(37)limt→∞⁡χt+tk;x0=limt→∞⁡χt;xtk=χt;y,which shows that (38)χ0;y∈DV⟹χt;y∈D∀t≥0,∀y∈DV.


## 8. Implementation of the Control Laws

In this section, we demonstrate the simulation results of a lead-carrier team of 2MMs transporting a random number of rigid objects in an obstacle-ridden workspace. The scenarios capture realistic situations to illustrate the effectiveness of the new control scheme and the centralized acceleration controllers. The section starts with a simple scenario and then considers teams with multiple carriers in the later scenarios.


### 8.1. Scenario 1

In this scenario, we consider a single lead-carrier pair carrying a rigid object, hence establishing and translating a globally rigid formation, while avoiding all obstacles in its trajectory. For simplicity and illustration purpose, we consider one leader and onlyone carrier. The final configuration that has the desired orientation of the pair is achieved through the use of ghost parking bays. The initial values and robot parameters are given below, while the values of each tuning parameter are summarized in Table 1. The control laws were implemented to generate feasible trajectories.Table 1
Scenario 1: tuning parameters.
Types
2MM 1
2MM 2
Workspace restrictionsfor s=1,…,8
α
1
s
=
0.01
α
2
s
=
0.01
Parking bay
ζ
11
=
ζ
12
=
ζ
13
=
0.01
ζ
21
=
ζ
22
=
ζ
23
=
0.01
Fixed obstaclefor m=1,2,3
γ
1
m
1
=
0.5
γ
2
m
1
=
0.5
Maximum interrobot bounds
α
12
=
0.1
α
21
=
0.1
Minimum interrobot bound
φ
12
=
0.1
φ
21
=
0.1
Mechanical singularities
ξ
11
=
ξ
12
=
0.5
ξ
21
=
ξ
22
=
0.5
ξ
13
=
ξ
14
=
ξ
15
=
0.01
ξ
23
=
ξ
24
=
ξ
25
=
0.01
Bounds on velocitiesfor r=1,…,4
β
1
r
=
0.1
β
2
r
=
0.1Figure5 shows the trajectories of the lead-carrier pair and we see that the prescribed globally rigid formation is maintained during the motion even when the pair approaches a fixed obstacle in its path. The behavior of the nonlinear centralized acceleration-based controllers of the lead-carrier is shown in Figure 6(a), for the wheeled platform and Figure 6(b) for the 2 links, indicating its inherent convergent nature.(1)
Robot parameters(m): l1=2, b1=1 and l2=l3=1.2.(2)
Angular positions (rad): θ11=θ21=0,θ12=-π/4,θ13=π/2,θ22=π/4,θ23=-π/2.(3)
Fixed obstacle(m): center is at (o11,o12)=(15,14), radius is ro1=2.(4)
Physical Limitations: maximum translational velocity is fixed at vmax=5 m/s. A maximum steering angle ϕmax=70° has been fixed. The maximum rotational velocities of links are as follows: ω1max=ω2max=1 rad/s.(5)
Clearance and safety parameters(m): ϵ1=ϵ2=0.1, ϵ3=0.3.(6)
Convergence parameters: δiv=20 for v=1,…,4 and i=1,2.(7)
Workspace boundaries(m): z1=η1=28, z2=η2=28.Figure 5
The resulting trajectories of 2MM teams transferring a rigid object. The positions of the lead and carrier robots are given as(x1,y1)=(5m,3m) and (x2,y2)=(5m,5m), respectively. The translational velocities are given as 3 m/s while the rotational velocities are π/360 rad/s. The targets are fixed at (p11,p12)=(24m,26.5m) and (p21,p22)=(26m,26.5m) with a radii of 0.5 m.Figure 6
The time evolution of the acceleration-based controllers of the lead-carrier shown in (a) for the wheeled platform and (b) for the 2 links when transporting a rigid object.
(a)
(b)


### 8.2. Scenarios 2 and 3

Scenarios 2 and 3 consider different lead-carrier teams with 3 and 4 carriers, respectively, maneuvering from initial to final states, while avoiding fixed and moving obstacles in their trajectories. The control laws were implemented to generate feasible trajectories. We note from Figures7(a) and 7(b) that the globally rigid formation of each lead-carrier pair and the locally rigid formation of the whole team are maintained en route the destination. The slight distortion of the team’s formation is observed when it approaches and avoids the fixed obstacles in the path. However, the original formation is reachieved after the avoidance of fixed obstacles, but with different bearing of the team. The changing rotation of the formation, because of the need for obstacle avoidance, has been achieved because of the new dϕ-strategy, which can also be seen to optimize the lane change and lane merge maneuvers.Figure 7
Scenarios 2 and 3. Evolution of trajectories for different lead-carrier teams. (a) and (b) have 3 (C1–C3) and 4 (C1–C4) carriers attached to the lead robot, respectively.
(a)
Scenario 2
(b)
Scenario 3


## 8.1. Scenario 1

In this scenario, we consider a single lead-carrier pair carrying a rigid object, hence establishing and translating a globally rigid formation, while avoiding all obstacles in its trajectory. For simplicity and illustration purpose, we consider one leader and onlyone carrier. The final configuration that has the desired orientation of the pair is achieved through the use of ghost parking bays. The initial values and robot parameters are given below, while the values of each tuning parameter are summarized in Table 1. The control laws were implemented to generate feasible trajectories.Table 1
Scenario 1: tuning parameters.
Types
2MM 1
2MM 2
Workspace restrictionsfor s=1,…,8
α
1
s
=
0.01
α
2
s
=
0.01
Parking bay
ζ
11
=
ζ
12
=
ζ
13
=
0.01
ζ
21
=
ζ
22
=
ζ
23
=
0.01
Fixed obstaclefor m=1,2,3
γ
1
m
1
=
0.5
γ
2
m
1
=
0.5
Maximum interrobot bounds
α
12
=
0.1
α
21
=
0.1
Minimum interrobot bound
φ
12
=
0.1
φ
21
=
0.1
Mechanical singularities
ξ
11
=
ξ
12
=
0.5
ξ
21
=
ξ
22
=
0.5
ξ
13
=
ξ
14
=
ξ
15
=
0.01
ξ
23
=
ξ
24
=
ξ
25
=
0.01
Bounds on velocitiesfor r=1,…,4
β
1
r
=
0.1
β
2
r
=
0.1Figure5 shows the trajectories of the lead-carrier pair and we see that the prescribed globally rigid formation is maintained during the motion even when the pair approaches a fixed obstacle in its path. The behavior of the nonlinear centralized acceleration-based controllers of the lead-carrier is shown in Figure 6(a), for the wheeled platform and Figure 6(b) for the 2 links, indicating its inherent convergent nature.(1)
Robot parameters(m): l1=2, b1=1 and l2=l3=1.2.(2)
Angular positions (rad): θ11=θ21=0,θ12=-π/4,θ13=π/2,θ22=π/4,θ23=-π/2.(3)
Fixed obstacle(m): center is at (o11,o12)=(15,14), radius is ro1=2.(4)
Physical Limitations: maximum translational velocity is fixed at vmax=5 m/s. A maximum steering angle ϕmax=70° has been fixed. The maximum rotational velocities of links are as follows: ω1max=ω2max=1 rad/s.(5)
Clearance and safety parameters(m): ϵ1=ϵ2=0.1, ϵ3=0.3.(6)
Convergence parameters: δiv=20 for v=1,…,4 and i=1,2.(7)
Workspace boundaries(m): z1=η1=28, z2=η2=28.Figure 5
The resulting trajectories of 2MM teams transferring a rigid object. The positions of the lead and carrier robots are given as(x1,y1)=(5m,3m) and (x2,y2)=(5m,5m), respectively. The translational velocities are given as 3 m/s while the rotational velocities are π/360 rad/s. The targets are fixed at (p11,p12)=(24m,26.5m) and (p21,p22)=(26m,26.5m) with a radii of 0.5 m.Figure 6
The time evolution of the acceleration-based controllers of the lead-carrier shown in (a) for the wheeled platform and (b) for the 2 links when transporting a rigid object.
(a)
(b)


## 8.2. Scenarios 2 and 3

Scenarios 2 and 3 consider different lead-carrier teams with 3 and 4 carriers, respectively, maneuvering from initial to final states, while avoiding fixed and moving obstacles in their trajectories. The control laws were implemented to generate feasible trajectories. We note from Figures7(a) and 7(b) that the globally rigid formation of each lead-carrier pair and the locally rigid formation of the whole team are maintained en route the destination. The slight distortion of the team’s formation is observed when it approaches and avoids the fixed obstacles in the path. However, the original formation is reachieved after the avoidance of fixed obstacles, but with different bearing of the team. The changing rotation of the formation, because of the need for obstacle avoidance, has been achieved because of the new dϕ-strategy, which can also be seen to optimize the lane change and lane merge maneuvers.Figure 7
Scenarios 2 and 3. Evolution of trajectories for different lead-carrier teams. (a) and (b) have 3 (C1–C3) and 4 (C1–C4) carriers attached to the lead robot, respectively.
(a)
Scenario 2
(b)
Scenario 3


## 9. Effect of Noise in Simulation

In this section, we consider the effect of noise in the control laws. Following the idea proposed in [31], we include the noise components into the obstacle avoidance functions and redefine the Lyapunov function as follows: (39)L=∑i=0nHi+Ri+Fi∑j=0j≠inαijMAij+σμij+φijMIij+σμij+ψijMOij+σμij+∑k=3i+13i+3ζikLSik+σρik+∑r=14βirUir+σνir+∑p=13ξipSip+σχip+∑m=13∑l=1qγimlFOiml+σϱiml+∑i=1n∑p=45FiξipSip+σχip.The terms μij,ρik,νir,χip, and ϱiml are time-dependent variables randomized between and including −1 and 1 and σ∈[0,1] is the noise level.Simulation. To include the effect of noise we have generated a simulation with a leader and 3 carriers having a rectangular formation as shown in Figure 8. We have also generated the graphs of the controller for the leader at various noise levels as shown in Figures 9–11, showing the effectiveness of the new set of controllers.Figure 8
Evolution of trajectories for a lead-carrier team consisting of one leader and three carriers. The corners of the boxes represent the position of the end-effectors of the leader and the three followers. Here we have a rectangular formation.Figure 9
Controllers for end-effector of leader withσ=0.
(a)
Controllersu01 in red and u02 in blue
(b)
Controllersu03 in red and u02 in blueFigure 10
Controllers for end-effector of leader withσ=0.1.
(a)
Controllersu01 in red and u02 in blue
(b)
Controllersu03 in red and u02 in blueFigure 11
Controllers for end-effector of leader withσ=0.2.
(a)
Controllersu01 in red and u02 in blue
(b)
Controllersu03 in red and u02 in blue


## 10. Conclusion and Future Work

This paper heralds a new set of centralized acceleration-based control laws that successfully tackles motion planning and dual-formation (locally rigid and globally rigid) control of a lead-carriers team of connected and autonomous 2-link mobile manipulators, in a priori known environment including our roads and highways. The overarching framework for the problem was the Lyapunov-based control scheme, which is essentially an artificial potential field method. The paper is a theoretical exposition into the applicability of the LbCS method. We restrict ourselves to showing the effectiveness of the motion planners and the control laws using simulations including the effect of noise.Adϕ-strategy is introduced to ensure virtual connectivity of the carrier robots to the lead robot during transportation of, but not limited to, large or multiple objects simultaneously on highways and roads, with safe and collision-free motion through V2X communications. This connectivity which has been built into the governing system of ODEs inherently ensures globally rigid formation between each lead-carrier pair of the team. On the other hand, a new recipe involving target configuration, dϕ-strategy, orientation consensus, and interrobot avoidances results in a locally rigid formation of the team. The new strategy also facilitates a rotation change of the formation, because of the need for obstacle avoidance, and this can be seen to optimize the lane change and lane merge maneuvers in heavy-traffic roads and highways.For the very first time, a dual-formation control problem of a lead-carrier team of 2-link mobile manipulators has been successfully solved, while the stability of the system has been proved in the sense of Lyapunov. While the swarm intelligence ensures energy efficiency and reduced costs, the new dual-formation ensures multitasking, job precision, and another solution to applications on roads such as convoying and payload transfer. In a nutshell, the centrally planned control algorithm designed in this paper demonstrates autonomy and to a certain extent the multitasking capabilities of the virtually connected collective in nature, while being fixed in more than one formation pattern. The methodology can be further developed to encompass practical considerations, such as wireless communications, to suit current proposed engineering solutions (to the problem of allowing connected and autonomous vehicles on public roads) that include a special lane equipped with sensors and wireless technologies to ensure adequate and timely V2I communication.



---

*Source: 9213805-2017-09-24.xml*