Potential Field Control of a Redundant Nonholonomic Mobile Manipulator with Corridor-Constrained Base Motion

Abstract

This work proposes a solution for redundant nonholonomic mobile manipulator control with corridor constraints on base motion. The proposed control strategy applies an artificial potential field for base navigation to achieve joint control with desired trajectory tracking of the end effector. The overall kinematic model is created by describing the nonholonomic mobile platform and the kinematics of the manipulator. The objective function used consists of a primary control task that optimizes the joint variables to achieve the desired pose or trajectory of the end effector and a secondary control task that optimizes the joint variables for the base to support the arm and stay within the corridor. As a last priority, an additional optimization is introduced to optimize the maneuverability index. The proposed baseline navigation has global convergence without local minima and is computationally efficient. This is achieved by an optimal grid-based search on a coarse discrete grid and a bilinear interpolation to obtain a continuous potential function and its gradient. The performance of the proposed control algorithm is illustrated by several simulations of a mobile manipulator model derived for a Pal Tiago mobile base and an Emiko Franka Panda robotic manipulator.

1. Introduction

In cases where the working range of industrial manipulators is insufficient, mobile manipulators are used, where the manipulator is attached to a mobile platform. The latter requires a more advanced localization and control algorithm. To achieve any pose of the end effector, a manipulator with at least 6 DOF (degrees of freedom) is required. However, in practice, manipulators and biological systems are usually equipped with more than six actuated joints to achieve redundancy. This allows them to circumvent motion constraints due to joint limits, avoid singularities, optimize the manipulability index, avoid obstacles, or introduce other tasks such as the mobile base preference path. For example, in this work, a redundant mobile manipulator system with 10 independent generalized coordinates (defining the configurations of the mobile platform and manipulator joints) is used to achieve an end-effector pose with 6 DOF. Due to the high degree of redundancy, the computation of inverse kinematics for such a robotic system becomes computationally intensive and complex. In general, there is no analytical solution, and we may be faced with an infinite number of different joint configurations that solve the inverse kinematics problem.

Usually, redundancy resolution in velocity space is achieved by the generalized Moore–Penrose inverse (pseudo-inverse), which minimizes the quadratic norm of joint rates for a desired translational and angular velocity of the end effector. The complexity of the pseudo-inverse of a manipulator Jacobian matrix is O(3) [1], which can become inefficient for hyper-redundant systems, such as snake-like robots or continuum robots [2]. Nevertheless, velocity pseudo-inverse approaches are popular because they provide universal, robot-independent, and well-documented solutions [3,4,5,6]. The approach relies on differentiable objective functions to obtain gradient joint velocities for redundancy resolution [7,8] and thus do not directly address the geometry of the redundancy degree, but instead address it at the velocity level. The latter becomes important when nonholonomic systems with wheels are used to achieve mobility. This leads to the distinction between kinematic redundancy and velocity redundancy [4], where the number of feasible velocities, and thus velocity redundancy, is reduced by the number of nonholonomic (velocity) constraints. Instead of a regular base with wheels, an omnidirectional base could be used that utilizes the unconstrained velocity space of the base, resulting in a holonomic system with simpler control design and higher velocity redundancy [9,10].

Various approaches that extend or replace the classical pseudo-inverse velocity approaches include reinforcement learning, particle swarm optimization [8,11,12]; nonlinear optimization approaches [7]; genetic algorithms [13], the model predictive approach [14,15]; neural networks [8], adaptive control [16]; the robust control approach [14,17]; graph search approaches, such as A star or rapidly exploring random tree [18,19]; potential field, and the like. A more detailed overview of planning and control approaches for mobile manipulators is presented in [5,20].

To achieve the desired performance of a mobile manipulator, the base and end effector must be planned and controlled together. Typically, this is achieved through pseudo-inverse velocity approaches, where the configuration of the base supports the desired performance of the end effector in tracking the trajectory [4]. In addition, a separate trajectory for the base can be provided as a second priority task [6,10,21], or the priorities between the base and the end-effector tracking can change depending on the nature of the task [22]. The position of the base can also be optimized by time relaxation along the intended path to better support the manipulator [23]. In [24], joint control of the base and end effector is proposed, with the arm also being nonholonomic. To deal with the uncertainties of base motion, an effective decoupling of the end effector and base motion is proposed in [25] using real-time control of the arm by fast visual inertial sensing.

In the literature, the main task of the mobile manipulator is to follow the desired effector trajectory. This can be achieved with decentralized control, where the mobile base and arm are separate controlled systems or with centralized systems where a single joint control system is used. The mobile base can be moved first in the arm’s work area before joint control begins, or the arm starts moving even before it is within the reaching area of the arm. The movement of the base may be free, moving only in the direction of the end-effector’s target, or it may attempt to optimize its own objective function, such as avoiding obstacles, but is not constrained to a path or trajectory. On the other hand, the base may be controlled to follow the reference trajectory, which may be a second priority task. The latter solution is very common in the literature [4,6,10,21] and requires planning of both the trajectories of the end effector and the base. In the presence of disturbances [26] or inadequate planning, the desired pose of the end effector may not be within the range of the mobile manipulator when the base is optimally following the trajectory or vice versa. This problem was explored in [23] with ways to relax the timing constraints on the mobile base, which can optimize its position along a fixed reference path.

Potential field approaches are widely used in mobile robotics for path planning and motion control. A common problem with potential field approaches is local minima, since a repulsive field is typically generated locally for an obstacle and can nullify attractive target behaviors. Full or partial obstacle information can be used to modify the potential field around obstacles to avoid local minima, as in [27,28,29] or by geometrically modeling obstacles through envelopes [30]. The desired change in the potential field to overcome local minima could be achieved by including multiple attractive points instead of just one point in the target [31] or by adding virtual obstacles to move away from local minima [32]. In addition, the artificial potential field can be enhanced by introducing a potential density along the obstacles for path planning using a quasi-geodesic curve, as presented in [33]. Strategies for bypassing local minima could also include behavioral strategies, such as wall tracking in [34].

Some approaches also combine global grid-based search algorithms with artificial potential fields to improve performance. A partial artificial potential field and a three-neighbor A* algorithm to address irregular obstacles is proposed in [35]. In [36], a combination between the probabilistic roadmap with A* search and the artificial potential field method is proposed to improve the roadmap construction. In [37], the authors combine intermediate goal points obtained by the A* algorithm and the improved potential field to obtain a global optimal path. The potential field can also be interpolated from a discrete cost map obtained from an optimal grid-based search [38,39].

Potential field approaches are also common in manipulators and mobile manipulators for path planning and control [5,40]. Redundant manipulator path planning is proposed in [41,42], where separate potential fields are proposed for translational and rotational motions. In [43], a deep reinforcement learning algorithm is combined with an artificial potential field to improve the convergence of 7 DOF manipulator planning. In [44], virtual targets and obstacles are added to avoid detected local minima in manipulator planning. Examples of collision-proof trajectory planning for mobile manipulators with artificial potential field can be found in [45,46].

This paper addresses the trajectory tracking problem for redundant mobile robots. The robot has 10 independent generalized coordinates that define the configuration of the mobile platform and joints to position the end effector in the global coordinate system with 6 DOF. The joint control of the end effector and the mobile base is implemented in velocity space using the classical pseudo-inverse Jacobian approach, which combines the coordinated action of the arm and the base considering the corridor-constrained motion of the base. The main idea is to allow free movement of the base within the given corridor so that the primary task of tracking the end-effector can be performed as efficiently as possible. In industry, such corridors are a common solution to enable efficient material flow of automated guided vehicles and other mobile transporters. The corridors are also usually marked with lines on the ground to prevent other participants from interfering with the operation of the autonomous transporters. To achieve the desired control behavior of the base, a navigation function is proposed. The navigation function is constructed from a known environment layout and enables convergent navigation of the base without problems with local minima.

The main contribution of this work is the following. We have derived a trajectory tracking controller for a redundant mobile platform with 9 DOF, consisting of a Pal Tiago Base and an Emika Franka Panda robotic manipulator. A velocity-based inverse kinematic approach is formulated and extended to account for the joint trajectory of the end effector and the mobile base for potential field navigation in a corridor-constrained environment. The proposed navigation function has no local minima because it applies an optimal discrete E*-graph-based search to compute the discrete cost-to-goal potential. To be efficient, a coarse discrete grid can be used since an additional interpolation using bilinear interpolation is provided to obtain a smooth potential and its gradient. Furthermore, several known approaches are integrated into a final solution applied to a redundant mobile manipulator. The performance is validated and compared using several simulations.

2. Problem Specification

The main task of the mobile manipulator is to follow the desired trajectory of the end effector, while the movement of the base is restricted by a corridor. It can move freely within the corridor so that optimal tracking of the end effector is achieved. In industry, such corridors are a common solution to achieve efficient material flow of automated guided vehicles and other mobile transporters. The basic idea is shown in Figure 1, in which the base can move freely between the forbidden gray areas at ground level ($z=0$ m) to best support the tracking of the end effector’s trajectory.

In this work, we propose potential field navigation for the base to stay within the designated corridor. The potential field is automatically computed based on the given layout and the known target position of the end effector (at the end of its reference trajectory) so that the base can be safely navigated. Note that the layout may also contain dead ends (e.g., the concave shape of the corridor in the top-left image in Figure 1), which can typically lead to local minima if the potential field is not properly designed. To obtain a feasible navigation (without local minima), the potential field is estimated using a graph-based search on a discrete grid, such as Dijstra’s, A*, E*, or a similar algorithm. This yields a discrete potential where each free cell contains unique distance-to-target values, and occupied cells have infinite values (gray cells in Figure 1 where base motion is not allowed). The values of the discrete potentials in Figure 1 (top right) were calculated using the E* algorithm [47], which extends the A* algorithm by adding two orthogonal parent nodes to obtain a better cost-to-goal estimate for each cell. Such a discrete potential field is computationally efficient because a relatively coarse discrete grid is used (e.g., 0.5 m cell border in Figure 1 top left), but it is not suitable for basic navigation because any position within the cell would have the same potential. To obtain a smooth potential field with unique values for any position, an interpolation approach could be used that takes into account the potential of the neighboring cells. Following the negative gradient (steepest descent), the mobile base can be safely and optimally navigated in the constrained space to support the end-effector’s movement without providing an explicit reference path for it. For known targets, the navigation functions can even be precomputed and easily used during operation. The following provides details on the construction of the potential field and simple gradient following control for the base.

3. Navigation Function for Corridor-Constrained Motion

The negative gradient of the potential function is used to navigate the robot to the target location where the potential is zero. However, for a given position of the mobile base within any free cell (white cells with cost-to-goal numbers in Figure 1 (top right)), equal potential is obtained which restricts the resolution of the estimated gradient descent. Therefore, we use bilinear interpolation as in [48] to obtain a smooth potential and its negative gradient estimate.

For a given mobile base position ${[x,y]}^T$ located in one cell, three additional neighboring cells are determined for interpolation so that the position lies within a rectangle connecting these four cells (see Figure 2).

Interpolated potential [49] at given point ${[x,y]}^T$ is defined as

$$P(x,y)=w_{00}{p}_{00}+w_{10}{p}_{10}+w_{01}{p}_{01}+w_{11}{p}_{11}$$

(1)

where $p_{ij}$ ($i,j\in \{0,1\}$) are discrete potentials, the weights $p_{ij}$ are defined by $w_{00}=(1-x_n)(1-y_n)$, $w_{10}=x_n(1-y_n)$, $w_{01}=(1-x_n)y_n$, $w_{11}=x_n{y}_n$, and $x_n=\frac{x-x_0}{d_c}$, $y_n=\frac{y-y_0}{d_c}$ are the normalized coordinates according to Figure 2.

The negative gradient of the interpolated potential $P(x,y)$ in ${[x,y]}^T$, the navigation for the safe path from anywhere in the environment to the target location (with potential 0), is derived as

$$\begin{array}{ccc}\hfill \mathbf{g}(x,y)& =\hfill & -\nabla P(x,y)=-{\left[\frac{\partial P(x,y)}{\partial x},\frac{\partial P(x,y)}{\partial y}\right]}^T=-\frac{1}{d_c}{\left[\frac{\partial P_n(x_n,y_n)}{\partial x_n},\frac{\partial P_n(x_n,y_n)}{\partial y_n}\right]}^T\hfill \\ \hfill & =\hfill & -\frac{1}{d_c}\left[\begin{array}{c}{p}_{11}{y}_n-p_{01}{y}_n+p_{00}(y_n-1)-p_{10}(y_n-1)\\ p_{11}{x}_n-p_{10}{x}_n+p_{00}(x_n-1)-p_{01}(x_n-1)\end{array}\right].\hfill \end{array}$$

(2)

Note that biliniear interpolation is a well-known technique in image resampling that operates on pixels that can be like cells in the robot environment layout (e.g., Figure 1). However, some of the four cells for interpolation may be forbidden and therefore have infinite potential since movement through them is not allowed. Therefore, the potential of these cells must be reconstructed before interpolating (1) or (2). This can be performed simply by examining the neighborhood of the eight cells and finding the free cell with the largest potential. The potential of the forbidden cell is then increased by the distance between the forbidden cell and the free cell with the largest potential. Similarly, cells could be reconstructed for interpolation outside the neighborhood boundary (with undefined potential). For more details on discrete potential adjustments for forbidden cells (gray cells in Figure 1) and environment borders, see [48].

The interpolated potential in (1) is continuous, but its gradient is discontinuous because bilinear interpolation is used. This is shown in the left part of Figure 3, where the gradient has discontinuities at the boundaries between the quadrants of a cell. Using such a gradient for navigation of the mobile base would also result in discontinuity of actions. Therefore, we propose an additional interpolation of the gradients to obtain a smooth gradient flow, as shown in the right half of Figure 3. To obtain an interpolated gradient, the same idea is used as for the interpolation of the potential in (1). First, the gradients for the centers of the four interpolating cells (the same four cells as in the interpolation of the potential) are estimated, which can be performed using the relation (2) by setting x, y to the coordinates of the cell centers.

Then, the interpolated gradient $\mathbf{h}(x,y)$ at the current position is obtained by

$$\mathbf{h}(x,y)=w_{00}{\mathbf{h}}_{00}+w_{01}{\mathbf{h}}_{01}+w_{10}{\mathbf{h}}_{10}+w_{11}{\mathbf{h}}_{11}$$

(3)

where the same weights $w_{00}$, $w_{01}$, $w_{10}$, $w_{11}$ as in (1) are used, and the estimated gradients of the cell centers are ${\mathbf{h}}_{00}$, ${\mathbf{h}}_{01}$, ${\mathbf{h}}_{10}$, and ${\mathbf{h}}_{11}$. The comparison between the gradient field $\mathbf{g}$ and the improved interpolated gradient $\mathbf{h}$ is shown in Figure 3.

4. Mobile Manipulator Model

The proposed joint trajectory tracking control of the end effector and mobile base corridor navigation using a potential field is illustrated by simulations on a nonholonomic and redundant mobile platform with 9 DOF. The platform consists of a Pal Tiago base and an Emika Franka Panda robotic manipulator (Figure 4). The manipulator is attached to the mobile base at a height of 0.833 m.

4.1. System Model

Denavit–Hartenberg (DH, with parameters in

Table 1. Denavit-Hartenberg (DH) for Emika Franka Panda manipulator.

Joint	1	2	3	4	5	6	7
$a_i$ [m]	0	0	0.0825	−0.0825	0	0.0880	0
$d_i$ [m]	0.333	0	0.316	0	0.384	0	0.1070
${\alpha }_i$ [rad]	−$\pi /2$	$\pi /2$	$\pi /2$	$-\pi /2$	$\pi /2$	$\pi /2$	0
${\theta }_i$ [rad]	${\theta }_1$	${\theta }_2$	${\theta }_3$	${\theta }_4$	${\theta }_5$	${\theta }_6$	${\theta }_7$

) notation is used to obtain direct kinematics of the manipulator

$${\mathbf{x}}_m=\mathbf{g}\left({\mathbf{q}}_m\right)$$

(4)

where ${\mathbf{x}}_m$ is the end effector pose expressed in local coordinates of the mobile base, ${\mathbf{q}}_m={[{\theta }_1,{\theta }_2,\cdots ,{\theta }_n]}^T$ is the configuration variable of the n-th DOF manipulator, and $\mathbf{g}(·)$ is a nonlinear mapping. The mobile base is constrained to move in a plane and has pose ${\mathbf{x}}_b={[x_b,y_b,{\varphi }_b]}^T$, so the configuration variable of the entire mobile platform extends to $\mathbf{q}={[x_b,y_b,{\varphi }_b,{\theta }_1,{\theta }_2,\cdots ,{\theta }_n]}^T$. Thus, the pose of the end effector ${\mathbf{x}}_e={[x,y,z,{\varphi }_x,{\varphi }_y,{\varphi }_z]}^T$, expressed in global coordinates, depends nonlinearly on $\mathbf{q}$

$${\mathbf{x}}_e=\mathbf{f}\left(\mathbf{q}\right).$$

(5)

The mobile base is assumed to have ideal rotation of the wheels, which prevents the base from moving in a lateral direction. This is a nonholonomic motion constraint that does not restrict the reachable space of ${\mathbf{x}}_b$, only its velocity space $\dot{{\mathbf{x}}_b}$, and will be considered in Section 4.2.

4.2. Velocity Kinematic Model

The time derivative of (5) yields the velocity dependence

$$\dot{{\mathbf{x}}_e}=\mathbf{J}\left(\mathbf{q}\right)\dot{\mathbf{q}}$$

(6)

where $J=\frac{\partial \mathbf{f}\left(\mathbf{q}\right)}{\partial \mathbf{q}}$ is the analytically determined Jacobian. Note that although the dimension of $\mathbf{q}$ is $n+3$ ($n=7$ for our manipulator), the whole system has $n+2$ degrees of freedom (DOF) in velocity space because the mobile base has a nonholonomic constraint ${\dot{x}}_{b}sin{\varphi }_b-{\dot{y}}_{b}cos{\varphi }_b=0$ (the base cannot slide laterally on the wheels). Therefore, the velocity of the end effector $\dot{{\mathbf{x}}_e}$ cannot be arbitrary but must take into account the differential drive kinematics of the base

$${\dot{\mathbf{x}}}_b={\mathbf{S}}_b\left(\mathbf{q}\right)\left[\begin{array}{c}v\\ \omega \end{array}\right]=\left[\begin{array}{cc}cos{\varphi }_b& 0\\ sin{\varphi }_b& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \omega \end{array}\right]$$

(7)

where v and $\omega$ are the command velocity for the base. Introducing pseudo-velocity of configuration variable $\zeta ={[v,\omega ,\dot{{\mathbf{q}}_m}]}^T={[v,\omega ,{\dot{\theta }}_1,{\dot{\theta }}_2,\cdots ,{\dot{\theta }}_n]}^T$ and taking into account the relations (6), (7), we obtain a feasible end-effector velocity taking into account the kinematic constraint of the base

$$\dot{{\mathbf{x}}_e}={\mathbf{J}}_C\left(\mathbf{q}\right)\zeta =\mathbf{J}\left(\mathbf{q}\right)\mathbf{S}\left(q\right)\zeta$$

(8)

where ${\mathbf{J}}_C\left(\mathbf{q}\right)=\mathbf{J}\left(\mathbf{q}\right)\mathbf{S}\left(q\right)$ is a constrained Jacobian of dimension $6\times (n+2)$ and

$$\mathbf{S}\left(\mathbf{q}\right)=\left[\begin{array}{cc}{\mathbf{S}}_b\left(\mathbf{q}\right)& 0_{3\times n}\\ 0_{n\times 2}& {\mathbf{I}}_{n\times n}\end{array}\right].$$

5. Inverse Kinematic in Velocity Space

Inverse kinematics are defined in velocity space and applied to the trajectory tracking control of the end effector.

5.1. Controller for the End Effector of the Mobile Manipulator

The mobile manipulator is redundant when $(n+2)>6$. The mobile manipulator in Figure 4 has $n+2=9$ degrees of freedom, which means that there are infinitely many solutions to the pseudo-velocities of the joints $\zeta$ that lead to the desired velocity of the end effector. This allows the mobile manipulator to achieve secondary objectives in addition to the desired velocities $\dot{{\mathbf{x}}_e}$.

The pseudo-velocities of the configuration vector $\zeta$ are obtained from the velocities of the end effector $\dot{{\mathbf{x}}_e}$

$$\zeta ={{\mathbf{J}}_C}^{+}{\dot{\mathbf{x}}}_e+\mathbf{N}{\zeta }_0$$

(9)

where ${{\mathbf{J}}_C}^{+}$ is the right pseudo-inverse of ${\mathbf{J}}_C$, $\mathbf{N}={\mathbf{I}}_{(n+2)\times (n+2)}-{{\mathbf{J}}_C}^{+}{\mathbf{J}}_C$ is the null space of ${\mathbf{J}}_C$, and ${\zeta }_0$ are pseudo-joint velocities for the lower priority task. The end-effector velocities are made proportional to the tracking error, and the feed-forward velocity of the reference trajectory is included to provide zero-error tracking of the reference trajectory

$${\dot{\mathbf{x}}}_e=\mathbf{K}\left({\mathbf{x}}_{ref}-{\mathbf{x}}_e\right)+{\dot{\mathbf{x}}}_{ref}$$

(10)

where ${\mathbf{x}}_{ref}$ is the reference pose for the end-effector, and $\mathbf{K}$ is a positive definite matrix of control gains.

Robust pseudo-inverse is used to mitigate kinematic singularity problems [3,22] with the proximity to the singularity limit estimated by the manipulability measure $w=\sqrt{det\left(\mathbf{J}{\mathbf{J}}^T\right)}$ as follows

$${{\mathbf{J}}_C}^{+}=\left\{\begin{array}{ccc}\hfill {\mathbf{J}}_C^T{\left({\mathbf{J}}_C{{\mathbf{J}}_C}^T+k_0{(1-w/w_0)}^2{\mathbf{I}}_{6\times 6}\right)}^{-1}& ,& w<w_0\hfill \\ \hfill {\mathbf{J}}_C^T{\left({\mathbf{J}}_C{{\mathbf{J}}_C}^T\right)}^{-1}& ,& w\ge w_0\hfill \end{array}\right.$$

(11)

where $w_0$ is the threshold value of the manipulability index, and $k_0$ is the scale constant.

Since the system is redundant, a lower priority task can also be provided where any ${\zeta }_0$ could be assigned. Given a scalar objective function $w\left(\mathbf{q}\right)$ that depends on the configuration $\mathbf{q}$, ${\zeta }_0$ can be defined to minimize $w\left(\mathbf{q}\right)$. To achieve a decrease of $w\left(\mathbf{q}\right)$, ${\zeta }_0$ must be chosen such that $\dot{w}\left(\mathbf{q}\right)\le 0$. The derivative of $w\left(\mathbf{q}\right)$ is

$$\dot{w}\left(\mathbf{q}\right)={\nabla }^{T}w\left(\mathbf{q}\right)\dot{q}={\nabla }^{T}w\left(\mathbf{q}\right)\mathbf{S}\mathbf{N}{\zeta }_0$$

(12)

where $\nabla w\left(\mathbf{q}\right)={[\frac{\partial w\left(\mathbf{q}\right)}{\partial {\mathbf{x}}_b},\frac{\partial w\left(\mathbf{q}\right)}{\partial {\mathbf{y}}_b},\frac{\partial w\left(\mathbf{q}\right)}{\partial {\varphi }_b},\frac{\partial w\left(\mathbf{q}\right)}{\partial {\varphi }_1},\cdots ,\frac{\partial w\left(\mathbf{q}\right)}{\partial {\varphi }_n}]}^T$, and relation $\dot{q}=\mathbf{S}\mathbf{N}{\zeta }_0$ is considered (see (8)). A possible choice for ${\zeta }_0$, as suggested by [4], is

$${\zeta }_0=-\lambda {\left({\nabla }^{T}w\left(\mathbf{q}\right)\mathbf{S}\mathbf{N}\right)}^T$$

(13)

where $\lambda$ is a positive scalar. The choice for ${\zeta }_0$ defined in (13) gives

$$\dot{w}\left(\mathbf{q}\right)=-\lambda {\nabla }^{T}w\left(\mathbf{q}\right)\mathbf{S}\mathbf{N}{\mathbf{N}}^T{\mathbf{S}}^T\nabla Tw\left(\mathbf{q}\right)\le 0$$

5.2. Secondary Task and Objective Function

In addition to the primary task in (9), a secondary task can also be provided to ensure that the base follows its desired planar trajectory. This can be achieved by a priority scheme [3,22] in which tracking the base’s reference trajectory has a lower priority than tracking the end effector

$$\zeta ={{\mathbf{J}}_C}^{+}{\dot{\mathbf{x}}}_e+\mathbf{N}{\mathbf{J}}_{C2}^{+}\left({\mathbf{v}}_b-{\mathbf{J}}_2{\mathbf{J}}_C^{+}{\dot{\mathbf{x}}}_e\right)+\mathbf{N}{\mathbf{N}}_2{\zeta }_0$$

(14)

where ${\dot{\mathbf{x}}}_e$ is the desired velocity of the end effector as the primary task, ${\mathbf{v}}_b={[v,\omega ]}^T={\mathbf{J}}_2\zeta$ is the desired base velocity vector of the secondary task, and ${\zeta }_0$ is the desired pseudo-velocities projected into the remaining orthogonal space. For the secondary task Jacobians ${\mathbf{J}}_2$, ${\mathbf{J}}_{C2}$, the pseudo-inverse Jacobian ${\mathbf{J}}_{C2}^{+}$ and the null space ${\mathbf{N}}_2$ hold: ${\mathbf{J}}_2\left(\mathbf{q}\right)=\left[\begin{array}{c}{\mathbf{I}}_{2\times 2},0_{2\times n}\end{array}\right]$, ${\mathbf{J}}_{C2}={\mathbf{J}}_2\mathbf{N}$, ${\mathbf{J}}_{C2}^{+}={\mathbf{J}}_{C2}^T{\left({\mathbf{J}}_{C2}{\mathbf{J}}_{C2}^T\right)}^{-1}$, and ${\mathbf{N}}_2={\mathbf{I}}_{9\times 9}-{{\mathbf{J}}_{C2}}^{+}{\mathbf{J}}_{C2}$.

Finally, the last priority task ${\zeta }_0$ is projected onto the remaining orthogonal subspace in (9) or (14), where system operation can be optimized according to an objective function. For the objective function, the manipulability measure (15) is used

$$w\left(\mathbf{q}\right)=\sqrt{det\left(\mathbf{J}\left(\mathbf{q}\right){\mathbf{J}}^T\left(\mathbf{q}\right)\right)}.$$

(15)

Note that lower priority tasks can only be executed if the system has sufficiently high redundancy.

5.3. Mobile Base Corridor-Constrained Control

The base can move freely in a defined corridor while assisting the end effector in tracking the reference trajectory. To achieve the desired movement of the base, the potential field navigation computed for the corridor is used. This defines the second priority task that guides the base away from the corridor boundaries and to the desired destination. Together with the primary task (end-effector tracking), this defines the joint control (14) where the overall commands for the base are defined.

For the base, we propose a negative gradient following control obtained from the proposed interpolated potential field and the interpolated gradient vector field (defined by (3)). Following the negative gradient guarantees safe motion toward the target with the base approaching the target in each control sample. Note that the potential function described in Section 3 has no local minima.

For current base state ${\mathbf{x}}_b={[x_b,y_b,{\varphi }_b]}^T$, the reference driving direction is defined by the negative gradient

$${\varphi }_{ref}=\angle \mathbf{h}(x_b,y_b)$$

(16)

the orientation error of the base reads

$$e_{{\varphi }_b}={\varphi }_{ref}-{\varphi }_b+2k\pi ;k=\underset{k^{\prime }=\{0,1,-1\}}{argmin}|{\phi }_{ref}-\phi +2k^{\prime }\pi |,$$

(17)

and the control law then defines the base velocities ${\mathbf{v}}_b={[v,\omega ]}^T$ as follows

$$\omega =K_{\omega }{e}_{{\varphi }_b}$$

(18)

$$v=v_{I}cos\left(e_{{\varphi }_b}\right)$$

(19)

where $K_{\omega }$ is a positive constant defining the closed loop dynamics of the orientation, and $v_I$ is the translational base velocity computed by the first priority task (the first element of ${{\mathbf{J}}_C}^{+}{\dot{\mathbf{x}}}_e$ in Equation (14)). The ${\mathbf{v}}_b={[v,\omega ]}^T$ thus obtained is applied to joint control (14).

6. Simulation Analysis and Validation

In the following, the control performance of the mobile manipulator is validated through several simulated scenarios. The control algorithm from Section 5 is implemented with a sampling time of $T_s=0.1$ s. Other constants used in the control are as follows. The gain matrix in (10) is $\mathbf{K}=2{\mathbf{I}}_{6\times 6}$, the parameters in (11) are ${\omega }_0=0.38$, $k_0=0.01$, and the gain in (18) is $K_{\omega }=3$. The results of trajectory tracking and pose control of mobile manipulator’s end effector are shown.

6.1. Joint Arm Trajectory Tracking and Base Potential Field Navigation

The arm must follow a predetermined reference trajectory that defines position and orientation (6 DOF). This is achieved as the first priority task in (14). The mobile base is navigated to the desired corridor using the potential field as a secondary task. Any remaining freedom in the redundancy is used to optimize the operation for manipulability. In the control law, the limits for velocities and accelerations are set to $\zeta ={[1.2,3,3,3,3,3,3,3,3]}^T$ and $\frac{d\zeta }{dt}={[1,2,2,2,2,2,2,2,2]}^T$, respectively.

In Figure 5, three different scenarios are shown: arm tracking in a free space where the base motion is not affected by tight corridor constraints (Figure 5, left column) and two cases of arm tracking where base motion must adapt to corridor constraints (Figure 5, middle and right columns). In Figure 6, the control velocities and tracking errors of the end effector are shown for scenarios in Figure 5.

In the last two scenarios, the reference trajectory intersects the corridor defined for the base. This is allowed since the corridor is constrained only for the base. However, the base and the arm must then find an appropriate configuration to allow the base to stay within the corridor and still support reference tracking of the end effector. Note how the base approaches the edge of the corridor and how the arm must extend toward the reference trajectory. At these moments, a slight increase in the velocities of the base and arm joints can be observed. As the working range of the arm is close to its limits, tracking errors also increase significantly.

To obtain a more realistic simulation, the scenario in the second column in Figure 5 is repeated in Figure 7 with added non-deterministic effects simulating noise and wheels slipping. Whenever the wheels of the mobile base slip, the end effector moves less or more than desired by the controller. The slippage also affects the accuracy of the sensor system (e.g., wheel odometry), leading to errors in the estimated system configuration [26]. In the simulation, the calculated command velocities are corrupted with additional randomly distributed normal noise. This results in disturbed motion of the base. The standard deviation for translation velocity noise is ${\sigma }_v=0.3$ m/s, and the standard deviation for angular velocity noise is ${\sigma }_{\omega }=0.3+0.4\left|\omega \right|$ rad/s. To mimic slip effects, the standard deviation for angular velocity increases linearly with angular velocity. The simulated slippage of the base causes a larger tracking error of the end effector. This is reflected in more aggressive control actions calculated to compensate for the tracking error, as shown in Figure 7. Despite the additional noise, the end effector reliably follows the reference trajectory.

6.2. Joint Arm Pose Control and Base Potential Field Navigation

The same control strategy can be used to reach the reference pose of the end effector without specifying an explicit trajectory to that reference pose. The mobile base is navigated similarly to Section 6.1 by the presented potential field navigation, which is used as a secondary task to support the arm. In control law, limits for velocities and accelerations are set to $\zeta ={[0.8,3,2,2,2,2,2,2,2]}^T$ and $\frac{d\zeta }{dt}={[1,2,2,2,2,2,2,2,2]}^T$, respectively.

In Figure 8, three different scenarios are shown: arm pose control in a free space where base motion is not affected by tight corridor constraints (Figure 8 left column) and two cases of arm pose control where base motion must adapt to corridor constraints (Figure 8 middle and right columns). To avoid unnecessary movements of the arm joints in Figure 8 and Figure 9 when the distance to the target position is large (e.g. larger than 1 m), the joint velocities are set to zero as shown in Figure 9.

7. Discussion

The discrete potential field (distance to-goal) can be precomputed for a known static environment and goal. If a mobile manipulator delivers between multiple known destinations (e.g., pickup and drop-off locations), a separate potential field is computed for each destination. Since a coarse discretization is possible (due to the interpolation used), this is a moderate memory requirement. At runtime, bilinear interpolation is performed only for the cells along the current path, resulting in a computationally efficient implementation. However, if the environment is dynamic (e.g., with moving obstacles), the discrete potential function can be updated at runtime. In this case, the version of E* with dynamic replanning should be used to achieve higher efficiency [47].

To compute the navigation function, the goal position for the base must be known. However, we only define corridor constraints for the base and have no explicit requirements for the path of the base or its goal. Therefore, the goal can be chosen the same as for the end effector. The actions for the base and the arm are then defined such that the end effector follows the reference trajectory (first priority task), and the base simultaneously travels along the current negative gradient of the navigation function (second priority task). The coordination of the tasks is implicitly archived since the secondary task lies in the orthogonal space of the primary one. However, this is only true as long as the reference trajectory can be reached from the corridor, taking into account the working range of the arm. If this is not the case, the fully extended arm cannot satisfy both control tasks (the primary and the secondary). This causes the manipulability index to approach zero, and thus the pseudo-inverse would have a singularity problem. With the robust pseudo-inverse (11), these singularity problems are mitigated, while at the same time accuracy is compromised, leading to increased end-effector tracking error.

8. Conclusions

In this work a solution is proposed for a redundant nonholonomic mobile manipulator with joint arm and base control, where corridor constraints are considered for the movement of the base. The base can move freely within the given corridor. A novel navigation function for base motion that has no local minima and optimally steers the base within the corridor is proposed. The navigation is computationally efficient and can be computed in advance for static environments. It applies optimal grid-based search to generate a discrete potential and online bilinear interpolation to obtain smooth potential and gradient fields.

In the future, we plan to extend the mobile base-gradient-following controller with model predictive capabilities to further improve the end-effector tracking. We will validate the proposed approach on a real system. It is also worthwhile to explore the possibility of extending the proposed potential field navigation to 3D corridors and also use it for arm navigation to avoid obstacles.