1. Introduction
Today, intelligent robots are widely used in shopping malls, restaurants, hospitals, and other places to assist people in completing tedious tasks. They can also be used to achieve robotic autonomous luggage trolley collection at airports, which is a major task at all airports [Reference Wang and Meng1]. Currently, most airports still manually collect and transfer luggage trolleys to designated areas, which is labor-intensive and relatively inefficient [Reference Wang and Meng2]. Using robots to collect and transfer dispersed luggage trolleys can improve work capacity [Reference Pan, Mai, Wang, Min, Wang, Cheng, Li, Lyu, Liu and Meng3]. The collection task can be divided into three sub-tasks: (1) Identifying and collecting dispersed and unoccupied luggage trolleys one by one [Reference Xiao, Luan, Zhao, Hong, Zhao, Chen, Wang and Meng4]; (2) Gathering the collected trolleys into a queue; and (3) Two collaborative robots transport the luggage trolley queue to a designated area for continued use by other passengers. The first two sub-tasks have been addressed by many researchers [Reference Wang and Meng1,Reference Xiao, Luan, Zhao, Hong, Zhao, Chen, Wang and Meng4]. Herein, this article focuses on the last challenging sub-task and proposes a divide-and-conquer control strategy that solves the multiple and complicated constraints separately. In this article, the control system we need to deal with consists of a luggage trolley queue and two collaborative robots, one as the leader and the other as the follower. Figure 1 illustrated a photo of the whole system.
The luggage trolley transportation system.
The luggage trolley queue can be regarded as a series of rigid bodies. Transporting a series of rigid bodies is commonly used in object manipulation, especially for a large quantity. However, it is not easy to handle the transportation of luggage trolley queues. The system is subject to nonholonomic constraints because collaborative robots have differential structures. In addition, the relative position of the leading and following robots must be considered to maintain a stable formation. Due to the complexity of the task, it is usually to calculate the feasible control input by means of optimization, which is widely used in a variety of complicated robot tasks [Reference Gilroy, Lau, Yang, Izaguirre, Biermayer, Xiao, Sun, Agrawal, Zeng, Li and Sreenath5–Reference Xiao, Tong, Yang, Zeng, Li and Sreenath7].
Inspired by the idea of divide-and-conquer, we propose a hierarchical policy to achieve this transportation task in this article. First, a high-level global planner provides a smooth path from initial to goal positions. Second, a low-level controller is designed to track the global path while maintaining the system’s formation. Specifically, we use Hybrid A* [Reference Dolgov, Thrun, Montemerlo and Diebel8] to generate a global path that is smooth enough and practical for the system to track. After that, we propose a divide-and-conquer control strategy that contains three modules. We use a model predictive controller module (MPC) to generate a set of initial control inputs. MPC can plan the trajectory by minimizing the loss [Reference Hu, Cui, Duan, Liu, Huang, Knoll and Chen9]. Subsequently, a safety filter, which consists of a discrete-time control Lyapunov function module (DT-CLF) and a control barrier function module (CBF) in a decentralized form, is employed to optimize the generated control inputs. The safety filter ensures that the control inputs satisfy all formation constraints in the collaborative transportation task. Finally, the filtered control inputs are sent to the actuators to drive the two collaborative robots to move the luggage trolley queue along the global path.
1.1. Related work
Manipulating objects with mobile robots has received a lot of attention recently. A substantial potential application is for object transportation [Reference An, Wu, Lin, Lin, Yoshinaga and Ji10]. However, existing challenges include but are not limited to mechanical design, planning algorithms, control strategies, etc. In this article, we pay attention to planning and control parts. Currently, the research works can be divided into two categories depending on the end effector. (1) Integrating robot manipulators on the mobile platform. (2) Designing customized manipulation mechanisms for specific tasks. The former is universal for sophisticated tasks represented by PR2 [Reference Bohren, Rusu, Jones, Marder-Eppstein, Pantofaru, Wise, Mösenlechner, Meeussen and Holzer11] and Fetch [Reference Wise, Ferguson, King, Diehr and Dymesich12]. Refs. [Reference Sirintuna, Giammarino and Ajoudani13] and [Reference Zhang, Sheng, Hu, Sheng, Xiong and Zhu14] also focuses on the control of collaborative mobile manipulators. However, the kinematic redundancy will complicate planning and control difficulties [Reference Hu, Liu, Zhang, Yi and Xiong15]. Another disadvantage is the high cost, which limits its widespread use. In contrast, the latter exhibits greater flexibility in specific tasks, which has also received extensive attention from academia and industrial community.
Most of the current research focuses on manipulating a single object by mobile robots. Both refs. [Reference Xiao, Luan, Zhao, Hong, Zhao, Chen, Wang and Meng4] and [Reference Wang, Mai, Ho, Liu, Li, Pan and Meng16] employ one mobile robot to collect a single luggage trolley. When dealing with more complex tasks, the functions and capabilities of a single robot are limited. Multi-robots gradually replace the single robot to achieve more complex work [Reference Juang, Lu and Huang17]. In our luggage trolley transport task, the connection between any adjacent two trolleys is not perfectly rigid, so the connection is unstable. Relying on push and pull in a single direction will not make the queue move as expected. In addition, a single robot cannot control the direction of the queue of luggage trolleys during transport. In order to solve the above problems, we adopt the mode of two robots, one in front and one in back, lining up with luggage trolleys and cooperating to complete the transportation.
Although [Reference Hu, Bhowmick and Lanzon18] realizes transporting multiple objects by six robots, the transported objects are not connected to any robots. Six robots form a cage that surrounds the objects and push them forward. This method does not control the payload well, especially for large objects. Ref. [Reference Koung, Kermorgant, Fantoni and Belouaer19] presents a four-robot fleet to transport a 20 kg plate. However, the payload is not caught by any robots. The connection between robots and the payload is unstable, especially during acceleration and deceleration. To solve the instability, we design a specific grasping device to fix the trolley. Ref. [Reference Hu, Liu, Zhang, Yi and Xiong15] realizes multi-robot collaborative handling of a deformable thin sheet. Since the object is deformable, the distance between robots and the angle of each robot does not need to be strictly constrained. Contrary to the flexibility of non-stationary applications and deformable objects, transporting a luggage trolley queue is much more difficult because there is little room for redundancy in the changing distance between two robots. The collaborative robots must clamp the luggage trolley queue to prevent disassembly. Additionally, due to the mechanical connection restrictions of the grasping device and the formation requirements of the system, there should be a maximum limit on the robot’s steering angle during the turning process. Therefore, we also need to control the angular constraints of the robot.
A well-known formation is the leader-follower formation. A popular communication type is implicit. The robots acquire information by interactions with the environment and other robots [Reference Tsiamis, Bechlioulis, Karras and Kyriakopoulos20]. The authors construct two mobile robots to transport the rod, which is also a rigid body. The authors implement the decentralized framework to improve the robustness. Ref. [Reference Wang and Schwager21] also makes followers apply force in the leader’s direction as a policy. Ref. [Reference Wang and Schwager22] introduces a consensus-based approach by using only local measurements from each robot. The authors demonstrate that leader-follower formation converges as long as the leader exists. In leader-follower formation, the leader has absolute leadership in the system. Only the leader knows the goal configuration of the task. The follower ensures contact with the object and the system formation only through force/torque measurements. Since the leader has no feedback from the follower, it cannot adjust accordingly when the follower is disturbed [Reference Hichri, Adouane, Fauroux, Mezouar and Doroftei23].
1.2. Our contributions
The contributions of this work are threefold.
-
1. We propose a divide-and-conquer control strategy to deal with the multiple constraints in luggage trolley transportation.
-
2. In the control strategy, a decentralized CBF for managing angle constraints is applied.
-
3. We verify the effectiveness of our system on both the simulation platform and the physical hardware platform.
The kinematic model is with nonholonomic constraints. The system contains two differential robots numbered as
$0$
(leading) and
$1$
(following), respectively. Inter-robot distance (
$L$
), system’s yaw angle
$\psi$
, and robot steering angles relative to the system (
$\varphi _0$
and
$\varphi _1$
) are also illustrated.
2. System description
2.1. Notation and problem definition
In this article, we use bold font lowercase letters for vectors, bold font capital letters for matrixes, regular font lowercase letters for scalars, and calligraphy font capital letters for spaces. We call each robot’s 2-D coordinates and 1-D yaw angle as state, and the whole system’s 2-D coordinates and 1-D yaw angle as pose. The collaborative transportation task has a 6-D state for two robots and a 3-D pose for the system as Fig. 2 shows. Let
$\mathcal{X} \subset \mathbb{R}^6$
be the state space of the two robots and
$\mathcal{P} \subset \mathbb{R}^3$
be the pose space of the whole system. We use
$\mathbf{x}\in \mathcal{X}$
and
$\mathbf{p}\in \mathcal{P}$
to represent the state of two robots and the pose of the system, respectively. They can be expressed as
\begin{equation} \mathbf{x} =\left [\begin{array}{l@{\quad}l@{\quad}l@{\quad}l@{\quad}l@{\quad}l}x_0, & y_0, & \theta _0, & x_1, & y_1, & \theta _1\end{array}\right ]^{\textrm{T}}, \end{equation}
\begin{equation} \mathbf{p} =\left [\begin{array}{l@{\quad}l@{\quad}l}\alpha, & \beta, & \psi \end{array}\right ]^{\textrm{T}}, \end{equation}
where
\begin{equation} \alpha =\frac{x_0+x_1}{2}, \quad \beta =\frac{y_0+y_1}{2}, \end{equation}
\begin{equation} \psi =\arctan 2\left (y_0-y_1, x_0-x_1\right ). \end{equation}
To describe
$\mathbf{x}$
, we denote
$k$
as time step and
$i$
,
$j$
as robot’s IDs specially, namely,
$\mathbf{x}_i=\left [\begin{array}{l@{\quad}l@{\quad}l}x_i,& y_i,& \theta _i\end{array}\right ] ^{\textrm{T}}$
. Let
$\mathcal{X} _{\textrm{obs}}\subset \mathcal{X}$
represent the obstacle space and
$\mathcal{X} _{\textrm{free}}=\mathcal{X} \setminus \mathcal{X} _{\textrm{obs}}$
be the free space. So does
$\mathcal{P} _{\textrm{obs}}\subset \mathcal{P}$
and
$\mathcal{P} _{\textrm{free}}\subset \mathcal{P}$
.
$\mathcal{P} _{\textrm{goal}}\subset \mathcal{P} _{\textrm{free}}$
is the goal space that can be expressed as a circle
$\left \{ \mathbf{x}\in \mathcal{X} _{\textrm{free}}\mid \left \| \mathbf{p}-\mathbf{p}_{\textrm{goal}} \right \| \le r \right \}$
with a radius of
$r$
.
$L$
is to represent the distance between two robots, where
$L=[\left ( x_0-x_1 \right ) ^2+\left ( y_0-y_1 \right ) ^2]^{1/2}$
. The control input space is
$\mathcal{U} \subset \mathbb{R} ^4$
, which consists of linear velocity
$v$
and angular velocity
$\omega$
for both robots. We use
$\mathbf{u}\in \mathcal{U}$
to be the control input of two robots as Eq. (3) shown,
\begin{equation} \mathbf{u}=\left [ \begin{matrix} v_0,& w_0,& v_1,& w_1\\ \end{matrix} \right ] ^{\textrm{T}}. \end{equation}
The kinematics is with the form of Eq. (4), where
$k$
is the discrete time step. The time interval of any adjacent two time steps is assumed as a constant,
$\Delta t$
,
\begin{equation} \mathbf{x}_{k+1}=\mathbf{x}_k + \mathbf{Bu}_k\Delta t. \end{equation}
Our problem is defined as follows: First, a real-time path planner should solve a feasible global path
$\mathbf{g}: \mathbb{R}\rightarrow \mathcal{P} _{\textrm{free}}$
from the initial pose
$\mathbf{g}_0 = \mathbf{p}_{\textrm{init}}$
to the goal
$\mathbf{g}_S \in \mathcal{P} _{\textrm{goal}}$
with step
$s\in \{0, 1, 2, \ldots, S\}$
. Let
$\mathcal{G}$
be the set that contains all the feasible paths. A cost function
$J(\mathbf{g}): \mathcal{G} \rightarrow \mathbb{R}$
, which maps the feasible path
$\mathbf{g}$
to a real number, is used to measure the path quality. The optimal path is formulated by
\begin{equation} \mathbf{g}^*=\underset{\mathbf{g}\in \mathcal{G}}{\textrm{arg}\min }J(\mathbf{g}). \end{equation}
Then, a real-time controller will generate a set of control inputs
$\mathbf{u}_k\in \mathcal{U}$
for two robots to track the optimal path while holding the distance of two robots to a reference distance (
$L\rightarrow L_{\textrm{ref}}$
) and limiting the steering angles of the two robots relative to the queue (
$|\varphi _i| \leq \varphi _{i,\max }$
).
$L_{\textrm{ref}}$
is decided by the number of trolleys in the queue, which can be measured when the queue forms, and
$\varphi _{i,\max }$
is the maximum steering angle of each robot relative to the system, which can be acquired by experiments. For any feasible path
$\mathbf{g} = \left [\begin{matrix}\mathbf{g}_0, \mathbf{g}_1, \mathbf{g}_2, \ldots, \mathbf{g}_S \end{matrix}\right ]$
, the controller must generate the control inputs to make the system traverse all the intermediate pose
$\mathbf{g}_s$
from
$\mathbf{g}_0$
to
$\mathbf{g}_S$
in turn, where
$s\in \{1, 2, 3, \ldots, S-1\}$
.
2.2. Preliminary work
We have independently developed two autonomous robots for luggage trolley queue transportation as shown in Fig. 3, which integrate moving chassis, grasping device, computing platform, control, and sensing modules. The two robots have similar structures and electron devices except for the position of grasping device, which is exclusively designed to secure the front and back of luggage trolleys, respectively. Similar robots can ensure the transportation of payloads.
The physical hardware consists of chassis, grasping device, computing platform, and sensor modules.
2.3. Localization
In this article, our robot formation is an approximate rigid body structure. A slight error in distance control (e.g.,
$>5{\,\textrm{cm}}$
) will cause the formation to fall apart. However, traditional localization methods, including but not limited to Lidar-based, such as AMCL [Reference Dellaert, Fox, Burgard and Thrun24], and vision-based, such as ORB-SLAM2 [Reference Mur-Artal and Tardós25] and its variants, cannot directly achieve the required accuracy. In addition, the localization uncertainty of a single robot will cause a more significant error in the distance measurement of robot formation. Therefore, in order to reduce the impact of localization problems in our system, we temporarily use Motion Capture System to provide an accurate positioning to verify the effectiveness of the control strategy.
3. Planning and control strategy
In this section, we will describe the system framework, as shown in Fig. 4, which consists of the global planning algorithm and the divide-and-conquer control strategy with a novel decentralized CBF. We call our robots the leader and follower for convenient description.
The framework of our system. The two collaborative robots run the programs with the same modules simultaneously. The location information is all from the Motion Capture System. The photo of two collaborative robots and the luggage trolley queue is from the top view.
3.1. Global planning
Considering that this task deals with a two-robot system with nonholonomic constraints that needs to maintain system formation, traditional path planning algorithms such as basic A* [Reference Hart, Nilsson and Raphael26] cannot handle the problems with nonholonomic constraints. We have also verified through simulation experiments that these methods are unsuitable for our objective. Therefore, we choose Hybrid A* [Reference Dolgov, Thrun, Montemerlo and Diebel8] algorithm, which considers the actual motion constraints of nonholonomic systems, as our global planning algorithm. Compared with basic A*, Hybrid A* has the following characteristics.
-
1. Hybrid A* is a heuristic search algorithm in the continuous coordinate system, and can ensure that the generated trajectory satisfies the vehicle nonholonomic constraint.
-
2. Hybrid A* takes the kinematic characteristics of the vehicle into account by constrained heuristics.
-
3. In order to improve the search speed and precision, Hybrid A* uses Reeds-Shepp model [Reference Reeds and Shepp27] to augment the search quality.
-
4. Hybrid A* uses the Voronoi field as the potential field to handle the navigability of narrow openings.
-
5. An optimization is used to smooth the final path.
Although the kinematics of our system is a little different from a vehicle, the path generated by Hybrid A* is still practical for our system by experimental verification. The path quality and comparison with basic A* will be illustrated in Section 4.1.1.
3.2. Control strategy
After obtaining the optimal path from the global planner, the next step is to achieve how to effectively track the path by two robots with the luggage trolley queue. On this basis, we also need to ensure the maintenance of system formation. We adopt a divide-and-conquer control strategy with the combination of three parts:
-
1. MPC: track the path generated by the global planner.
-
2. DT-CLF: maintain the distance between two robots, namely, to maintain the formation.
-
3. CBF: limit the steering angles of two robots, also to hold the system formation.
In this article, we call the latter two modules together as the safety filter. The functions of the three modules are basically decoupled from each other. Compared with integrating them in a single controller, this divide-and-conquer way has two advantages: higher solution efficiency and larger solution probability. We have tried to fuse them in one optimization problem and found the result unsatisfactory. Sometimes, it needed to add slacks to reduce constraints for solving a feasible solution. Finally, we adopt the mode of splitting into three modules. All nonlinear programs are formulated in CasADi [Reference Andersson, Gillis, Horn, Rawlings and Diehl28] and solved by IPOPT [Reference Wächter and Biegler29].
3.2.1. MPC
Considering the two robots as a whole, we adopt the discrete model. The kinematic model is shown in Fig. 2 and Eq. (6),
\begin{equation} \mathbf{x}_{k+1}=\mathbf{x}_k+\mathbf{Bu_k}\Delta t, \end{equation}
\begin{equation} \mathbf{x}_k=\left [\begin{array}{l@{\quad}l@{\quad}l@{\quad}l@{\quad}l@{\quad}l}x_{0,k}, & y_{0,k}, & \theta _{0,k}, & x_{1,k}, & y_{1,k}, & \theta _{1,k}\end{array}\right ]^{\textrm{T}}, \end{equation}
\begin{equation} \mathbf{B}=\left [ \begin{array}{c@{\quad}c@{\quad}c@{\quad}c} \cos \left ( \theta _{0,k} \right )& 0& 0& 0\\[3pt] \sin \left ( \theta _{0,k} \right )& 0& 0& 0\\[3pt] 0& 1& 0& 0\\[3pt] 0& 0& \cos \left ( \theta _{1,k} \right )& 0\\[3pt] 0& 0& \sin \left ( \theta _{1,k} \right )& 0\\[3pt] 0& 0& 0& 1\\ \end{array} \right ], \end{equation}
\begin{equation}\mathbf{u_k}=\left [\begin{array}{l@{\quad}l@{\quad}l@{\quad}l}v_{0,k},& w_{0,k},& v_{1,k},& w_{1,k} \end{array}\right ] ^{\textrm{T}}. \end{equation}
In this phase, we pay more attention to make the system’s center track the global plan during movement. The constraints on formation control are not strict. Only the last predictive state MPC is required to maintain the distance as
$L_{\textrm{ref}}$
. For other states, there is no distance constraint. This not only gives the system a tendency to maintain distance requirements but also avoids problems such as the increase in computing time and infeasible solutions caused by strong constraints. This tendency also ensures that the solution of MPC will not make the two-robot system deviate too far away from the formation, which alleviates the pressure of solving the following modules to a certain extent.
The tracking can be interpreted as an optimization problem. The objective is to minimize the weighted sum of the square of tracking error,
$\left \| \mathbf{p}_N-\mathbf{g}_m \right \| _{\mathbf{Q}_{\text{MPC}}}^{2}$
, and the square of control inputs
$\sum _{k=0}^{N-1}{\left \| \mathbf{u}_k \right \| _{\mathbf{R}_{\text{MPC}}}^{2}}$
, where
$N$
is the predictive horizon of MPC,
$ \mathbf{p}_N$
is the last pose of MPC prediction,
$\mathbf{g}_m$
is the tracked pose on the global path,
$\mathbf{Q}_{\text{MPC}}$
and
$\mathbf{R}_{\text{MPC}}$
are the positive definite weight matrixes. For selecting
$\mathbf{g}_m$
, first the pose
$\mathbf{g}_o$
, which is the nearest to the system’s current pose, is chosen. Then,
$\mathbf{g}_m = \mathbf{g}_{o+s}$
can be calculated by a certain step
$s$
. The Euclidean distance between
$\mathbf{g}_m$
and
$ \mathbf{p}_N$
is calculated. Minimizing control inputs to the objective is to control the system by a set of control inputs that are as small as possible. The relevant hard constraints are added to limit the maximum value of linear velocity
$v$
and angular velocity
$\omega$
. Additionally, as mentioned before, we want MPC to express a tendency that holds the system’s formation while avoiding too much pressure on the solver in this phase, so we just make a hard constraint that the last state prediction must maintain the reference distance strictly, which means
$L_N = L_{\text{ref}}$
, where
$L_N$
and
$L_{\text{ref}}$
are the inter-robot distance of the
$N$
-th state and reference inter-robot distance respectively. The final formulation of MPC is
\begin{equation} \min _{\mathbf{u}} \quad \left \| \mathbf{p}_N-\mathbf{g}_m \right \| _{\mathbf{Q}_{\text{MPC}}}^{2}+\sum _{k=0}^{N-1}{\left \| \mathbf{u}_k \right \|_{\mathbf{R}_{\text{MPC}}}^2}, \end{equation}
\begin{equation} \textrm{s.t.} \quad -\mathbf{u}_{k,\max }\le \mathbf{u}_k\le \mathbf{u}_{k,\max }, \end{equation}
\begin{equation}\mathbf{u}_k\in \mathcal{U}, \end{equation}
\begin{equation} L_N = L_{\text{ref}}, \end{equation}
\begin{equation} k\in \left \{ 0,1,2,\ldots,N-1 \right \}, \end{equation}
where
$\mathbf{u}_k\in \mathbf{U}=\left [ \begin{matrix}\mathbf{u}_0,\mathbf{u}_1, \mathbf{u}_2, \ldots, \mathbf{u}_{N-1}\end{matrix}\right ]$
. Angle constraints are not taken into account in this module. The predicted control input is
$\mathbf{U} \subset \mathbb{R}^{4\times N}$
. We call
$\mathbf{u}_0$
as
$\mathbf{u}_{\text{MPC}}$
and send it to the next module.
3.2.2. DT-CLF
The previous step calculates the control input mainly for tracking the global path, while the constraints that guarantee the system’s formation are ignored. Although we add a distance constraint on the last predictive state, it only maintains a converging tendency, which is insufficient in practical application. Stronger guarantees on the stabilization of the inter-robot distance are wanted. We leverage DT-CLF to meet the distance requirement.
Definition 1 (DT-CLF, based on ref. [Reference Agrawal and Sreenath30]). A map
$V: \mathcal{X} \rightarrow \mathbb{R}$
is an exponential control Lyapunov function for the discrete-time control system of Eq. (
4
) if there exist positive constants
$\kappa _{1}$
,
$\kappa _{2}$
,
$\kappa _3$
and a control input
$\mathbf u_{k}: \mathcal{X} \rightarrow \mathbb{U}, \forall \mathbf x_{k} \in \mathcal{X}$
such that
\begin{equation} \kappa _{1}\left \|\mathbf x_{k}\right \|^{2} \leq V\left (\mathbf x_{k}\right ) \leq \kappa _{2}\left \|\mathbf x_{k}\right \|^{2}, \end{equation}
and
\begin{equation} \Delta V\left (\mathbf x_{k}, \mathbf u_{k}\right ) + \kappa _3 V\left (\mathbf x_{k}\right ) \leq 0, \end{equation}
where
$\Delta V\left (\mathbf{x}_k, \mathbf{u}_k\right ) \triangleq V\left (\mathbf{x}_{k+1}\right )-V\left (\mathbf{x}_k\right )$
with
$\mathbf{x}_{k+1}$
following Eq. (
4
).
We focus on the error between the inter-robot distance and its desired value:
\begin{align} & e_k \triangleq \mathbf{x}^{\textrm{T}} \mathbf D^{\textrm{T}} \mathbf D \mathbf{x} - L_{\text{ref}}^2 \notag \\ &= (x_{0,k}-x_{1,k})^2 + (y_{0,k}-y_{1,k})^2 - L_{\text{ref}}^2, \end{align}
with
\begin{equation} \mathbf D = \left[\begin{array}{c@{\quad}c@{\quad}c@{\quad}c@{\quad}c@{\quad}c} 1 & 0 & 0 & -1 & 0 & 0\\[4pt] 0 & 1 & 0 & 0 & -1 & 0 \end{array}\right]. \end{equation}
Then, we design the candidate DT-CLF for distance stabilization as
\begin{equation} V(e_k)\triangleq \left ( e_k \right )^2. \end{equation}
Obviously, Eq. (12) satisfies Eq. (8) by
$0 < \kappa _1 < 1$
and
$\kappa _2 > 1$
. To render Eq. (12) as a valid DT-CLF, we pose the following CLF constraint for our dual-robot system Eq. (6) to ensure the origin is exponentially stable:
\begin{equation} \Delta V\left (e_k, \mathbf{u}_k\right )+c V\left (e_k\right ) \leq 0, \end{equation}
where
$\mathbf{x}_{k+1}$
(associated with
$e_{k+1}$
) follows the robots’ kinematics Eq. (6).
We expect to modify
$\mathbf{u}_{\text{MPC}}$
as slightly as possible while holding the DT-CLF constraints to enable both path tracking and distance maintenance. Therefore, the optimization in this phase is formulated as
\begin{equation} \min _{\mathbf{u}} \left \| \mathbf{u}-\mathbf{u}_{\text{MPC}}\right \|^{2}_{\mathbf{R}_{\text{CLF}}}, \end{equation}
\begin{equation} \textrm{s.t.} \quad \Delta V(e,\mathbf{u}) + c V(e)\le 0, \end{equation}
\begin{equation} -\mathbf{u}_{\max }\le \mathbf{u}\le \mathbf{u}_{\max }, \end{equation}
\begin{equation} \mathbf{u}\in \mathcal{U}. \end{equation}
We call the optimization result as
$\mathbf{u}_{\text{CLF}}$
and sent it to the next module.
3.2.3. CBF
Considering the above-mentioned mechanical designs of the robot, there exists a physical limit for the angle between the grasping device and the robot chassis. Respecting this angle constraint is essential for the system’s safe and successful operation. For practical consideration while without loss of theoretical guarantees, we use CBF and formulate a quadratic programming (CBF-QP) [Reference Ames, Coogan, Egerstedt, Notomista, Sreenath and Tabuada31,Reference Ames, Xu, Grizzle and Tabuada32] to ensure the system’s safety. In this subsection, we shall first construct the CBFs for our system and then pose the optimization problem. In addition, we present a distributed derivation toward the optimization problem for the sake of scalability.
Definition 2 (CBF, based on ref. [Reference Ames, Coogan, Egerstedt, Notomista, Sreenath and Tabuada31]). Let
$\mathcal C \subset \mathcal X\subset \mathbb R^n$
be the superlevel set of a continuously differentiable function
$h\,:\,\mathcal X \to \mathbb R$
, then
$h$
is a control barrier function (CBF) if it satisfies
\begin{equation} \dot h (\mathbf x, \mathbf u) \ge -\gamma h(\mathbf x) \end{equation}
for a constant
$\gamma > 0$
and for all
$\mathbf x \in \mathcal X$
.
We first formulate the CBFs for the system concerning the angle constraints. As shown in Fig. 2, we denote the angle between the luggage trolley queue and each robot as
$\varphi _i$
for
$i\in \{0,1\}$
. The safe operating zones of these angles are
$-\varphi _{i, \max } \leq \varphi _i \leq \varphi _{i, \max } \,, i\in \{0, 1\}$
. For each robot, we choose the safety value function
$h_i:\,\mathcal{X} \to \mathbb R$
as
\begin{equation} h_i(\mathbf x) \triangleq \frac{1}{2}\left (\varphi _{i, \max }^2-\varphi _i^2\right ). \end{equation}
Accordingly, the safe set for the entire system reads as the intersection of the zero-superlevel sets of the continuously differentiable functions
$h_i$
for
$i\in \{0,1\}$
:
\begin{equation} \mathcal{C} \triangleq \bigcap _{i\in \{0,1\}} \left \{\mathbf{x} \in \mathcal{X}_{\text{free}}\,:\, h_i(\mathbf x) \geq 0 \right \}. \end{equation}
Without loss of generality, we take robot
$i$
as an example. To make
$h_i$
a valid CBF, we shall ensure the following:
\begin{equation} \dot{h}_i+\gamma _i h_i \geq 0, \end{equation}
where
$\gamma _i$
is a positive constant. Differentiating
$h_i$
with respect to time, we get
\begin{equation} \begin{split} \dot{h}_i&=-\varphi _i\dot{\varphi }_i \\ &=-\varphi _i\left ( \dot{\psi }-\dot{\theta }_i \right ) \\ &=-\varphi _i\left ({\frac{{\partial \psi }}{\partial \mathbf{x}_i}}^{\textrm{T}}\dot{\mathbf{x}}_i+{\frac{{\partial \psi }}{\partial \mathbf{x}_j}}^{\textrm{T}}\dot{\mathbf{x}}_j-\omega _i \right ). \end{split} \end{equation}
where the first partial is associated with robot
$i$
’s state and movements and the second relates to the other robot
$j$
’s movements. Naturally, the angle between the robot
$i$
and the luggage trolley queue is also up to the other robot for that the other robot is simultaneously manipulating the queue as well. The first partial in the parenthesis of Eq. (19) is
\begin{equation} \frac{\partial \psi }{\partial \mathbf{x}_i}=\left [ \begin{matrix} \displaystyle{\frac{y_j-y_i}{L^2}},& \displaystyle{\frac{x_i-x_j}{L^2}},& 0\\ \end{matrix} \right ]^{\textrm{T}}. \end{equation}
Further combining the differential-drive model of each robot yields
\begin{equation} \begin{split}{\frac{\partial \psi }{\partial \mathbf{x}_i}}^{\textrm{T}}\dot{\mathbf{x}}_i&= \left [ \begin{matrix} \displaystyle{\frac{y_j-y_i}{L^2}},& \displaystyle{\frac{x_i-x_j}{L^2}},& 0\\ \end{matrix} \right ] \left [ \begin{array}{c} v_i\cos \theta _i\\ v_i\sin \theta _i\\ \omega _i\\ \end{array} \right ] \\ &=\frac{v_i}{L^2}\left ( \left ( y_j-y_i \right ) \cos \theta _i+\left ( x_i-x_j \right ) \sin \theta _i \right ) \\ &=\frac{v_i}{L^2}\cdot \overrightarrow{L}^{-\bot }\cdot \overrightarrow{\theta _i} \\ &=\frac{v_i}{L}\cdot \overrightarrow{\psi }^{-\bot }\cdot \overrightarrow{\theta _i}, \end{split} \end{equation}
where
$\overrightarrow{L} = L\left [\begin{array}{l@{\quad}l}\cos (\psi ), \sin (\psi )\end{array}\right ]^{\textrm{T}}$
,
$\overrightarrow{\theta _i}=\left [\begin{array}{l@{\quad}l}\cos (\theta _i), \sin (\theta _i)\end{array}\right ]^{\textrm{T}}$
,
$\overrightarrow{\psi }=\left [\begin{array}{l@{\quad}l}\cos (\psi ), \sin (\psi )\end{array}\right ]^{\textrm{T}}$
,
$z^{-\perp }$
denotes rotating a planar vector
$z$
by
$-\dfrac{\pi }{2}$
rad, and
$z^{\perp }$
means a
$\dfrac{\pi }{2}$
rotation. In a similar spirit, the second term in the parenthesis of Eq. (19) is derived as
\begin{equation} {\frac{\partial \psi }{\partial \mathbf{x}_j}}^{\textrm{T}} \dot{\mathbf{x}}_j=\frac{v_j}{L} \cdot{\overrightarrow{\psi }^{\perp }} \cdot \overrightarrow{\theta _{j}}. \end{equation}
Taking Eqs. (21) and (22) back into Eq. (19), we have
\begin{equation} \dot{h}_i=\frac{\varphi _i}{L}\left [ \begin{array}{c} -\overrightarrow{{\psi }}^{-\bot }\cdot \overrightarrow{\theta _i}\\ L\\ \end{array} \right ] ^{\textrm{T}}\left [ \begin{array}{c} v_i\\ \omega _i\\ \end{array} \right ]+\frac{\varphi _i}{L}\left [ \begin{array}{c} -\overrightarrow{{\psi }}^{\bot }\cdot \overrightarrow{\theta _j}\\ 0\\ \end{array} \right ] ^{\textrm{T}}\left [ \begin{array}{c} v_j\\ \omega _j\\ \end{array} \right ]. \end{equation}
Now, we have the expression of
$\dot{h}_i$
. Substituting Eq. (23) into Eq. (18), we have
\begin{equation} \frac{\varphi _i}{L}\left [ \begin{array}{c} -\overrightarrow{{\psi }}^{-\bot }\cdot \overrightarrow{\theta _i}\\ L\\ \end{array} \right ] ^{\textrm{T}}\left [ \begin{array}{c} v_i\\ \omega _i\\ \end{array} \right ]+\frac{\varphi _i}{L}\left [ \begin{array}{c} -\overrightarrow{{\psi }}^{\bot }\cdot \overrightarrow{\theta _j}\\ 0\\ \end{array} \right ] ^{\textrm{T}}\left [ \begin{array}{c} v_j\\ \omega _j\\ \end{array} \right ] + \gamma _i h_i \geq 0. \end{equation}
For notation’s brevity, we further simplify Eq. (23) by defining
$\mathbf A_{i} \triangleq \left [\mathbf{A}_{i i}^{\textrm{T}},\, \mathbf A_{ij}^{\textrm{T}}\right ]$
, wherein
\begin{equation} \mathbf A_{i i} \triangleq \frac{\varphi _i}{L}\left [\begin{array}{c}\overrightarrow{\psi }^{-\perp } \cdot \overrightarrow{\theta _i} \\ -L \end{array}\right ], \end{equation}
\begin{equation} \mathbf A_{i j} \triangleq \frac{\varphi _i}{L}\left [\begin{array}{c}\overrightarrow{\psi }^{\perp } \cdot \overrightarrow{\theta _j} \\ 0\end{array}\right ]. \end{equation}
Then, we can write condition Eq. (24) as a linear constraint with respect to control
\begin{equation} \mathbf A_{i} \mathbf{u} \leq \gamma _i h_i. \end{equation}
Finally, by applying the CBF constraints of all robots, we can formulate the filter as an optimization problem, resulting in a CBF-QP:
\begin{align} \min _{\mathbf{u}}\quad \left \|\mathbf{u}-\mathbf{u}_{\text{CLF}}\right \|^2_{\textbf{R}_{\text{CBF}}}\!, \end{align}
\begin{equation} \textrm{s.t.}\quad \mathbf A_{i} \mathbf{u} \leq \gamma _i h_i, \quad \forall i \neq j, \end{equation}
\begin{equation} -\mathbf{u}_{\max }\le \mathbf{u}\le \mathbf{u}_{\max }, \end{equation}
\begin{equation} \mathbf{u} \in \mathcal{U}, \end{equation}
where
$\mathbf{u}_{\text{CLF}}$
is the filtered control input provided in Section 3.2.2.
Observing Eq. (27), one can see that the optimization is in a centralized sense because it involves all of the robots’ states and controls. This optimization formulation scales poorly with respect to the number of robots and thus is not a good choice for real-time execution. Inspired by ref. [Reference Wang, Ames and Egerstedt33], we shall resolve this problem by splitting the constraints so the CBF-QP can be solved distributedly by each robot without compromising the theoretical guarantees.
Here we define
$\mathbf{u}_i \in \mathcal{U}_i$
for each robot analogically, where
$\mathcal{U}_i \subset \mathbb{R}^2$
is the control input space for each robot. Splitting Eq. (27b), for each coupled pair of robots
$i$
and
$j$
(there is only one in our case), we have
\begin{equation} \mathbf A_{i i}^{\textrm{T}} \mathbf{u}_i + \mathbf A_{i j}^{\textrm{T}} \mathbf{u}_j \leq \gamma _i h_i. \end{equation}
Similarly, for CBF of robot
$j$
, we have the analogous separation with
\begin{equation} \mathbf A_{j i}^{\textrm{T}} \mathbf{u}_i + \mathbf A_{j j}^{\textrm{T}} \mathbf{u}_j \leq \gamma _j h_j. \end{equation}
Further separating terms with respect to different robots’ controls yields
\begin{equation} \mathbf A_{i i}^{\textrm{T}} \mathbf{u}_i \leq \frac{\eta _{i i}}{\eta _{i i}+\eta _{i j}} \gamma _i h_i, \end{equation}
\begin{equation} \mathbf A_{j i}^{\textrm{T}} \mathbf{u}_i \leq \frac{\eta _{j i}}{\eta _{j i}+\eta _{j j}} \gamma _j h_j, \end{equation}
for robot
$i$
, and
\begin{equation} \mathbf A_{i j}^{\textrm{T}} \mathbf{u}_j \leq \frac{\eta _{i j}}{\eta _{i j}+\eta _{i i}} \gamma _i h_i, \end{equation}
\begin{equation} \mathbf A_{j j}^{\textrm{T}} \mathbf{u}_j\leq \frac{\eta _{j j}}{\eta _{j j}+\eta _{j i}} \gamma _j h_j, \end{equation}
for robot
$j$
, where
$\eta _{**}$
are positive parameters. One can always verify that adding up terms from both sides of Eqs. (30a) and (31a) yields Eq. (28), and adding Eqs. (30b) and (31b) yields Eq. (29). For simplicity, we denote
\begin{equation} \gamma _{i i} \triangleq \frac{\eta _{i i}}{\eta _{i i}+\eta _{i j}} \gamma _i, \end{equation}
\begin{equation} \gamma _{j i} \triangleq \frac{\eta _{j i}}{\eta _{j i}+\eta _{j j}} \gamma _j. \end{equation}
Therefore, the decentralized CBF-QP for each robot to solve will bes
\begin{align} \min _{\mathbf{u}_i } \quad &\! \left \|\mathbf{u}_i-\mathbf{u}_{\text{CLF},i}\right \|^2_{\mathbf{R}_{\text{CBF}, i}}, \end{align}
\begin{align} \text{ s.t. } \quad & A_{i i} \mathbf{u}_i \leq \gamma _{i i} h_i, \end{align}
\begin{align} & A_{j i} \mathbf{u}_i \leq \gamma _{j i} h_j, \end{align}
\begin{align}&\! -\mathbf{u}_{i,\max }\le \mathbf{u}_i\le \mathbf{u}_{i,\max }, \end{align}
\begin{align} &\mathbf{u}_i \in \mathcal{U}_i. \end{align}
After being filtered by DT-CLF and CBF, the control input commands
$\mathbf{u}_{\text{safe}}$
will be ultimately sent to the corresponding actuators.
4. Experiments
In this section, the experiment results of the proposed control strategy as well as the high-level global planner are shown. The experiments consist of two parts. The first part is to verify the smoothness of the path solved by the global planner. The second part is to verify the formation maintenance of inter-robot distance and angles by control strategy while tracking the global path. The former is completed on the simulation platform, while the latter is completed on both the simulation and physical hardware platforms. For the physical experiments, we design three representative path shapes constructed by different initial and goal positions: straight line, right-angle curve, and U-shape curve. These shapes are the dominating path conditions that the robots mainly deal with in real-world scenarios.
In order to facilitate the readers who have interests to reproduce our work, Table I provides the value of pivotal parameters and the corresponding description. Because the two robots share the same parameters, for brevity, we use robot
$i$
to represent.
The value of pivotal parameters.
4.1. Simulation experiments
4.1.1. Planning
We use Hybrid A* to plan a path that conforms to the nonholonomic constraints of the system. Compared with A*, the path is much smoother and with better performability. We use a right-angle curve for demonstration. The visualization of path quality is illustrated in Fig. 5(a) and (b). Obviously, Hybrid A* provides a more viable path. Figure 5(b) is the path that we use in simulation experiments. The computing time is also acceptable, which is under 100 ms on our computing platform. For the physical experiments, the leading robot must face back to the following one to grasp the luggage trolley queue as shown in Fig. 1. We also adjust the frame of the leading robot in simulation correspondingly.
Simulation experiment results. For distance maintenance, the mean and standard deviation are indicated. For angle maintenance, the safe region is shown with the boundary and green shadow.
4.1.2. Control strategy
Figure 5(c) and (d) demonstrate the distance and angle maintenance of the control strategy. The distance between the two robots remains obviously stable near the reference, and the error (
$<2$
cm) is acceptable. The mean and standard deviation of distance maintenance error are calculated by the absolute value of each sample in case the positive errors counteract the negative errors. In the following parts, the processes of distance maintenance error are the same. The steering angles of the leader and follower remain in the safe region consistently. The computing time of the safety filter is 21 ms, which is comparable to the MPC computing time of 22 ms. That is also what we are looking for. Figure 5(e) and (f) show the ultimate control inputs.
4.2. Physical hardware experiments
We implement the proposed planning algorithm and control strategy on physical hardware robots. The robot-mounted computing platform is NUC with Intel i7-1165G7 CPU and 16-GB RAM. Each transported luggage trolley is with
$4$
omnidirectional wheels. The weight is 15 kg and the length is
$790$
mm for each. We put five luggage trolleys into a queue for physical experiments. The bond of adjacent trolleys is just by the trolley-mounted fastener without any other additional connection. The overall length of our system is more than
$2$
m. For activation, we use warm-up, which accelerates the robots to the specified control input in two seconds. The experiment results are shown in Fig. 6 and the detailed experimental design is shown as follows.
Physical experiment results : (a)-(e) are for the straight line, (f)-(j) are for the right-angle curve, and (k)-(o) are for the U-shape curve. For each case, the global path, distance, and angle maintenance effectiveness, ultimate control inputs of
$v$
and
$\omega$
are provided. For distance maintenance and angle maintenance, the processes are the same as Fig. 5.
4.2.1. Straight line
The system moves along a straight line shown in Fig. 6(a), which is the easiest and the most common path. The proposed method achieves a distance maintenance error of less than
$2.5$
cm as Fig. 6(b) shows. Figure 6(c) angle constraints bring almost no impact in this case. Only the distance constraints will influence system formation. Figure 6(d) and (e) illustrate the control inputs of this case.
4.2.2. Right-angle curve
The system turns a right-angle curve as shown in Fig. 6(f). Distance constraints are also dominated because the rotation angle is relatively small. Distance error is under
$1.5$
cm as presented in Fig. 6(g). Figure 6(h) shows that the steering angles of two robots are also limited in the safe region obviously. Figure 6(i) and (j) illustrate the control inputs of this case.
4.2.3. U-shape curve
The system turns a U-shape curve with about
$150^{\circ }$
shown in Fig. 6(k), which is the most difficult. Due to the nonholonomic constraints and mechanical limitations, the system will deconstruct if the steering angles exceed the maximum values. According to experiment measurement, the maximum steering angle is about
$60^\circ$
for both robots. In view of the safety margin, we set the maximum value as
$50^\circ$
in CBF. We have tried that if CBF is removed, the system will fall apart when tracking this U-shape path. Figure 6(l) demonstrates the distance maintenance error is under
$1.8$
cm. Attractively, the robot steering angles are still located in the safe region as shown in Fig. 6(m), although the path curve is very sharp compared with the system length. The leader’s steering angle presses close to the boundary because it wants to turn. However, CBF guarantees that it will never exceed the safe region and further guarantees the system’s formation. Figure 6(n) and (o) illustrate the control inputs of this case.
4.3. Summary
Both the simulation and physical hardware experiments show the effectiveness of the proposed method. The collaborative robots can drive the luggage trolley queue to move from the initial pose to the goal region via a smooth path. While moving, the collaborative robots can maintain the system’s formation. The distance maintenance error is less than
$2.5$
cm for all the conditions. The steering angles for the robots are always maintained in the safe region. The errors and computing times are all acceptable.
5. Conclusion and future work
This article proposes a divide-and-conquer control strategy for transporting a luggage trolley queue. For the multi constraints in this problem, we decouple and solve them one by one. Before the controller, we use Hybrid A* to generate a global path that considers the nonholonomic constraints. Then, we use a real-time MPC to track the global path. Followed by a safety filter, which consists of DT-CLF and CBF, the controller will output a
$\mathbf{u}_{\text{safe}}$
that can drive the system move to the designated goal region while holding the distance and angle constraints. We verified the effectiveness of our method through both the simulation platform and the physical hardware platform.
Subject to the precise localization of the Motion Capture System, we cannot do physical hardware experiments in a larger environment. Therefore, we will focus on state estimation to enhance the localization performance of Lidar and extend our experimental scenarios to more complex environments in future work. Additionally, avoiding pedestrians and obstacles in dynamic environments is also the next stage of our work.
Author contributions
Xuheng Gao and Hao Luan conceived and designed this work. Hao Luan set up the simulation platform. Xuheng Gao and Bingyi Xia conducted the physical hardware experiments. Ziqi Zhao designed the hardware. Xuheng Gao and Hao Luan wrote this article. Jiankun Wang and Max Q.-H. Meng provided guidance and supervision.
Financial support
This research is supported by ShenZhen Key Laboratory of Robotic Perception and Intelligence (ZDSYS20200810171800001), National Natural Science Foundation of China Grant #62103181, and Shenzhen Outstanding Scientific and Technological Innovation Talents Training Project under Grant RCBS20221008093305007.
Competing interests
The authors declare no Competing interest exist.
Ethical approval
Not applicable.
