Development of Novel Motion Planning and Controls for a Series of Physically Connected Robots in Dynamic Environments

Abstract

The motion planning and control of physically connected robots, e.g., docked mobile robots (DMR), is a challenging problem due to robots’ underactuated and nonlinear dynamics as well as their physical constraints. The majority of the motion planning approaches developed for DMR are not robust to robots’ mass difference and are only applicable to static environments. This paper proposes a novel motion planning methodology for a DMR, consisting of a nonholonomic circular-shape leader and N holonomic passive-wheels active-joints circular-shape followers, through developing: (i) a trajectory planner to determine the motion of docked followers for tracking a desired path, (ii) a robust motion controller to navigate DMR through the planned trajectory, and (iii) a collision avoidance strategy to provide a collision-free transit for followers in dynamic environments. We propose two novel approaches for collision avoidance: a reactive approach which is a decentralized method that utilizes robots’ on-board measurement sensors, and a cooperative approach which is a centralized approach that uses environment information (e.g. obstacle locations) to prevent imminent collisions. In the reactive approach, the collision avoidance strategy is comprised of two control laws, one for obstacle avoidance and the other for satisfying joint constraints. In the cooperative approach, the collision avoidance strategy replans a collision-free trajectory for docked followers, and then deploys our trajectory planner and motion controller to navigate the DMR through the trajectory. The performance of the proposed reactive and cooperative approaches were shown and compared through simulations as well as implementation in a virtual robot experimentation platform (V-REP). The results showed that while the reactive approach is more efficient in terms of computation time and energy consumption, the cooperative approach requires less lateral deviation for avoiding the obstacles which is beneficial for operation in confined spaces. We finally compared our motion planning methodology with other existing methods in the literature. This comparison proved that our method is applicable to complex paths in dynamic environments, scalable with the number of followers, robust to various robots’ masses, and more computationally efficient.

Appendix

Using the Lagrangian approach, the matrices $\boldsymbol {M}$, $\boldsymbol {V}$, $\boldsymbol {B}$, $\boldsymbol {J}$ and $\boldsymbol {A}$ in Eq. 2 are given by Eqs. 35 and 36:

$$ \boldsymbol{V}= \left[\begin{array}{ccccccccc} 0 & 0 & h_{0}\text{c}_{0}\dot{\theta}_{0} {M_{1}^{N}} & h_{1}\text{c}_{1}\dot{\theta}_{1} (m_{1}+ 2{M_{2}^{N}}) & {\ldots} & h_{k}\text{c}_{k}\dot{\theta}_{k} \left( m_{k}+ 2M_{k + 1}^{N}\right) & {\ldots} & h_{N-1}\text{c}_{N-1}\dot{\theta}_{N-1} (m_{N-1}+ 2 m_{N}) & h_{N}\text{c}_{N}\dot{\theta}_{N} m_{N}\\ 0 & 0 & h_{0}\text{s}_{0}\dot{\theta}_{0} {M_{1}^{N}} & h_{1}\text{s}_{1}\dot{\theta}_{1} (m_{1}+ 2{M_{2}^{N}}) & {\ldots} & h_{k}\text{s}_{k}\dot{\theta}_{k} \left( m_{k}+ 2M_{k + 1}^{N}\right) & {\ldots} & h_{N-1}\text{s}_{N-1}\dot{\theta}_{N-1} (m_{N-1}+ 2 m_{N}) & h_{N}\text{s}_{N}\dot{\theta}_{N} m_{N}\\ 0 & 0 & 0 & 0 & {\ldots} & 0 & {\ldots} & 0 & 0\\ \colon & \colon & \colon & \colon & {\ddots} & \colon & \colon & \colon & \colon\\ 0 & 0 & h_{0}h_{l}\text{s}_{0l}\dot{\theta}_{0} (m_{l}+ 2 M_{1}^{l-1}) & h_{1}h_{l}\text{s}_{1l}\dot{\theta}_{1} (m_{l}+m_{1}+ 2 M_{2}^{l-1}) & {\ldots} & h_{k}h_{l}\text{s}_{kl}\dot{\theta}_{k} \left( m_{l}+m_{k}+ 2M_{k + 1}^{l-1}\right) & {\ldots} & 0 & 0\\ \colon & \colon & \colon & \colon & {\ddots} & \colon & \ddots & \colon & \colon\\ 0 & 0 & h_{0}h_{N-1}\text{s}_{0(N-1)}\dot{\theta}_{0} \left( m_{N-1}+ 2 M_{1}^{N-2} \right) & h_{1}h_{N-1}\text{s}_{1(N-1)}\dot{\theta}_{1} \left( m_{1}+m_{N-1}+ 2M_{2}^{N-2}\right) & {\ldots} & h_{k} h_{N-1}\text{s}_{k(N-1)}\dot{\theta}_{k} \left( m_{k}+m_{N-1}+ 2M_{k + 1}^{N-2}\right) & {\ldots} & 0 & 0\\ 0 & 0 & h_{0}h_{N}\text{s}_{0N}\dot{\theta}_{0} M_{1}^{N-1} & h_{1}h_{N}\text{s}_{1N}\dot{\theta}_{1} (m_{1}+ 2{M_{2}^{N}}) & {\ldots} & h_{k}h_{N}\text{s}_{kN}\dot{\theta}_{k} \left( m_{k}+ 2M_{k + 1}^{N}\right) & {\ldots} & h_{N-1}h_{N} \text{s}_{(N-1)N}\dot{\theta}_{N-1} (m_{N-1}+ 2m_{N}) & 0 \end{array}\right] $$

(35)

$$\begin{array}{@{}rcl@{}} \boldsymbol{M} &=& \setlength{\arraycolsep}{\arraycolsep} \left[\begin{array}{ccccccc} a_{11} & 0 & a_{13} & {\ldots} & a_{1(k + 3)} & {\ldots} & a_{1(N + 3)}\\ 0 & a_{22} & a_{23} & {\ldots} & a_{2(k + 3)} & {\ldots} & a_{2(N + 3)}\\ a_{31} & a_{32} & a_{33} & {\ldots} & 0 & {\ldots} & 0 \\ \colon & \colon & \colon & {\ddots} & \colon & \colon & \colon \\ a_{(l + 3)1} & a_{(l + 3)2} & a_{(l + 3)3} & {\ldots} & a_{(l + 3)(k + 3)} & {\ldots} & 0 \\ \colon & \colon & \colon & {\ddots} & \colon & \colon & \colon \\ a_{(N + 3)1} & a_{(N + 3)2} & a_{(N + 3)3} & {\ldots} & a_{(N + 3)(k + 3)} & {\ldots} & a_{(N + 3)(N + 3)} \end{array}\right] \\ \boldsymbol{B}&=&\frac{1}{R_{w}} \left[\begin{array}{lll} \text{c}_{0}&\text{c}_{0}& {0}_{3\times N}\\ \text{s}_{0}&\text{s}_{0}&\\ b_{0}&-b_{0}&\\ {0}_{N\times 2} & R_{w} \mathbb{I}_{N\times N} \end{array}\right],\\ \boldsymbol{J}&=& {\displaystyle \left[\begin{array}{cc} c_{0}& \\ s_{0}& {0}_{2\times (N + 1)} \\ {0}_{(N + 1)\times 1} & \mathbb{I}_{(N + 1)\times (N + 1)} \end{array}\right]},\\ \boldsymbol{A}&=& \left[\begin{array}{lllll} \text{s}_{0}&-\text{c}_{0}& 0 & \ldots&0 \end{array}\right] \end{array} $$

(36)

in which

$$\begin{array}{@{}rcl@{}} &&a_{11} = \sum{_{i = 0}^{N}}m_{i}, \qquad\qquad\qquad\qquad\qquad~ a_{13}= h_{0} s_{0} {M_{1}^{N}} \\ &&a_{22} = \sum{_{i = 0}^{N}}m_{i}, \qquad\qquad\qquad\qquad\qquad~ a_{23}= -h_{0} c_{0} {M_{1}^{N}}, \\ &&a_{1(k + 3)}= h_{k} s_{k} \left( m_{k}+ 2M_{k + 1}^{N}\right), \qquad\quad a_{1(N + 3)}= h_{N} s_{N} m_{N} , \\ &&a_{2(k + 3)}= -h_{k} c_{k} \left( m_{k}+ 2M_{k + 1}^{N}\right), \quad\quad a_{2(N + 3)}= -h_{N} c_{N} m_{N},\\ &&a_{(l + 3)1} = -h_{l} s_{l} \left( m_{l}+ 2M_{0}^{l-1}\right), \qquad\quad a_{31} = -h_{0} m_{0} s_{0}, \\ &&a_{(l + 3)2} = h_{l} c_{l} \left( m_{l}+ 2M_{0}^{l-1}\right), \qquad~~\quad a_{32} = h_{0} m_{0} c_{0},\\ &&a_{(l + 3)3}= -h_{l} h_{0} c_{0l} \left( m_{l}+ 2M_{1}^{l-1}\right), ~~\quad a_{33}= I_{0},\\ &&a_{(N + 3)1}= -h_{N} s_{N} M_{0}^{N-1}, \qquad\qquad~~\quad a_{(N + 3)2}= h_{N} c_{N} M_{0}^{N-1},\\ &&a_{(N + 3)3}= -h_{N} h_{0} c_{0N}M_{1}^{N-1}, \qquad\qquad a_{(N + 3)(N + 3)}= I_{N},\\ &&a_{(l + 3)(k + 3)}= -h_{l} h_{k} c_{kl} \left( m_{l}+m_{k}+ 2M_{k + 1}^{l-1}\right), \\ &&a_{(N + 3)(k + 3)}= -h_{N} h_{k} c_{kN} \left( m_{k}+M_{k + 1}^{N}\right),\\ \end{array} $$

and $\mathbb {I}$ is the identity matrix, $\text {c}_{0}= \cos \theta _{0}$, $\text {s}_{0}= \sin \theta _{0}$, $\text {c}_{lk}= \cos (\theta _{l}-\theta _{k})$, $\text {s}_{lk}= \sin (\theta _{l}-\theta _{k})$, $\text {c}_{l}= \cos \theta _{l}$, $\text {s}_{l}= \sin \theta _{l}$ and ${M_{l}^{k}}={\sum }_{i=l}^{k} m_{i}$ for l,k = 0,…,N. As shown in Fig. 1, $b_{0}$ and $R_{w}$ are the radius of leader and of its driving wheels, respectively. Also, $m_{i}$ and $I_{i}$ are respectively the mass and inertia of the i th robot and its docking links for $i = 0,\ldots ,N$.