1 Introduction

Autonomous vehicles (AVs), that are capable of carrying task without supervision or teleoperation, and remotely operated vehicles (ROVs) are mainly used in applications where human access would imply high risk of danger or there exists limited access. They are mainly used for remote inspection and monitoring, with suitable electro–mechanical structures for remote repair, maintenance or material handling (Lozano-Perez, 2012). Interest in autonomous manipulation is constantly increasing, as performing manned tasks in unsafe conditions is not only dangerous, but also costly, as professional experts with extensive training are needed to perform such operations (see, e.g., Ruggiero, Lippiello, & Ollero, 2018).

Designing a safe and robust robotic vehicle manipulator system (VMS) that can accomplish manipulation in unknown conditions, however, is a challenging task from control point of view as the overall system has highly coupled non-linear dynamics along with the parametric uncertainties. Navigation within unstructured environment is difficult (Lozano-Perez, 2012). Further, grasping and manipulation of an object contains its own challenges since the system can be imposed to unstable dynamics (Colgate & Hogan, 1988). In addition, these robots, particularly for underwater and aerial applications, need to manipulate the environment without a stationary base. Motion controller for such systems requires the capability of simultaneously stable localization, trajectory tracking, and stable physical interaction with the environment (Andaluz et al., 2012).

To overcome these challenges, some studies consider a single kinematic/dynamic model for the overall system such that the robotic arm mounted on a mobile platform is defined and controlled as a coupled system, resulting in a complex overall model, but enhancing the redundancy at the task space (see, e.g., De Luca et al., 2006; Lippiello & Ruggiero, 2012). Similar to this approach, the overall movement of mobile manipulators can be planned to carry out sub-tasks such as singularity avoidance, maximization of applicable force in desired directions, and manipulability enhancement. Manipulability is defined as the measure of a robotic system’s ability to adjust its end-effector’s position/orientation, (see Yoshikawa, 1985). In Yamamoto and Yun (1994), for instance, dynamic-coupling is presented such that mobile vehicle localizes itself to enhance manipulability of the robotic arm mounted on it; (see Yamamoto & Yun, 1996) for its extension encapsulating dynamic interaction between the vehicle and manipulator. A robust adaptive controller was proposed in Andaluz et al. (2012) based on a combined system dynamics for the mobile manipulators to tackle the point stabilization and trajectory tracking problems while increasing the manipulability of the robotic arm.

To reduce modelling and control complexities one can alternatively design independent controllers for each robotic system, the vehicle and the manipulator, and define different control objectives for each sub-system. In this approach any vehicle motion is considered to cause a perturbation in the task space and needs to be attenuated by the manipulator (see, e.g., Barbalata, Dunnigan, & Petillot, 2018; Ram, Pathak, & Junco, 2019). In Rigotti-Thompson et al. (2020), for a skid-steer mobile manipulator, the end-effector motion is decoupled from the base motion to compensate disturbed motions on uneven terrains via implementing an \({\mathcal {H}}_{\infty }\) controller with/without feed-forward control actions.

The position of the vehicle relative to the task, on the other hand, determines the effectiveness of the robotic arm during the task execution. Energy efficiency, singularity avoidance, and dexterity have been used as the main criteria desired to be improved. Due to the non-availability of the objective function’s gradient, it is a common approach to use metaheuristic algorithms for the base position optimization such as swarm intelligence and genetic algorithm (Dang & Nguyen, 2016; Mitsi et al., 2008; Ren et al., 2017). In Carriker et al. (1991), motion planning of a mobile manipulator is considered as an optimization problem where the vehicle position and manipulator joint angles were considered separately and optimum points, based on the end-effector position and force/torque, were estimated via simulated annealing. A decoupled approach has been established for base position optimization in Yu et al. (2018) where an internal penalty function and generalized Lagrange multiplier methods were utilized for obtaining near-optimal positions while nominating the positioning, orientation, and singularity avoidance as the constraints. Sparse uniform grid decomposition and sequential quadratic programming methods were implemented for base position optimization in Fan et al. (2021) while considering joint range, joint speed, singularity avoidance and collision avoidance as search constraints. Prior studies on optimum positioning, however, have not demonstrated stable physical interaction with unknown objects under external disturbances. In our paper, we realize optimum positioning for maximum force transmission for physical interaction with environment.

The effect of the manipulator’s motion on the vehicle is negligible in the ground based vehicles as the base is stationary. In the underwater and aerial applications, when the manipulator has relatively small mass with respect to the vehicle, the effects of the manipulator motion on base vehicle positioning are not significant and there fore a decoupled control approach can be adapted for a less complex and effective control (Barbalata et al., 2018). Decoupled control approach has also the advantage of easily accommodating different control cycle rates for the base vehicle and the robot arm manipulator. Thus, in this study, we adapt the decoupling approach and define different tasks for the vehicle and the robotic arm mounted on it; the vehicle moves to the optimum position to provide maximum manoeuvrability, (avoiding joint singularity and enhancing effort transmission from the joints to a task space) to the robot arm and the arm carries out the manipulation while attenuating any disturbances at the task space. We apply particle swarm optimization (PSO) to find an optimum vehicle position such that the force/torque transmission and velocity control capacities of the robotic arm are increased in pre-defined task directions, see Fig. 1.

Fig. 1
figure 1

The vehicle can be relocated at optimum position based on the manipulation direction such that effort transmission and fine velocity control properties of the robotic arm is enhanced for a given task

Full size image

In this paper, the primary aim is to enhance the force transmission of a manipulator at task space via optimal base localization while maintaining velocity control for task execution. For our method, we inspire from the observation that humans adopt their body posture and position while performing a force interaction task in order to apply effective force with their arms. Imitating this behaviour, the strength capabilities of a mobile robotic arm could be increased to accomplish manipulations that require excessive effort, such as for the initial drive to turn a valve. While doing this, task space position-force control and joint singularity problems should also be addressed for a stable manipulation under environmental disturbances. The presented control and optimization methods that we used in our overall system are as follows:

  • Reset controller for attenuating any vehicle motion at the task space; i.e., contact point stabilization,

  • Admittance controller for establishing a safe interaction with the environment,

  • Forward dynamic controller for eliminating any excessive joint motions while the robotic arm is close to the singular joint configurations,

  • PSO for optimum localization of the vehicle to enhance the force transmission and fine velocity control properties of the mobile robotic arm.

To the best of our knowledge, this is the first research attempt to implement directional manipulability along with a metaheuristic optimization algorithm to extend interaction capabilities of a mobile manipulator. Valve turning, similar to a wheel rotation, in underwater environmental conditions is considered as a case study for the experimental evaluation (see, e.g., Di Lillo et al., 2020; Korpela, Orsag, & Oh, 2014). The contributions of this paper can be summarised as

  • Metaheuristic optimisation method is combined with control architectures to maintain optimal vehicle position while interacting with unknown objects under environmental disturbances.

  • Maximum force transmission is achieved by adopting directional manipulability with the optimisation algorithm, where the algorithm finds the robot configuration that will provide maximum force in selected task space directions.

  • The optimisation and control frameworks are demonstrated to transmit maximum force with a physical system that can emulate any (land, underwater, or aerial) VMS, under environmental disturbances.

1.1 Notation

Bold lower-case letters, \(\varvec{x}\), represent the column vectors, non-bold upper case-letters, X, denote matrices, non-bold lower case-letters, x, represent single variables, and \(^{\top }\) denotes the transpose. The transformation from a vector to a skew-symmetric matrix is denoted by \([]_{\times }\) operator. The identify matrix is denoted as \({\text {I}}\).

2 Methods

2.1 Reference frames and transformations

When combining different sensors and motion frames, it is required to utilize a single frame of reference for each controlled motion. The force/torque sensor, for instance, provides measurements in sensor frame (s) which differs from the end-effector frame (e) and more importantly needs to be transferred to the contact/task space (t) as that is the main concern during an interaction, see Fig. 2 for the frame of references in this study.

Fig. 2
figure 2

Motion’s and sensors’ frames of references

Full size image

The force/torque sensor measures the force applied by the environment on the end-effector (\(\varvec{f}_s \!=\!\begin{bmatrix} f^x_s&f^y_s&f^z_s&\tau ^x_s&\tau ^y_s&\tau ^z_s \end{bmatrix}^{\top }\)); thus, the force applied by the manipulator on the environment \(\varvec{f}_e\) is obtained by taking the negative of the refined (eliminating sensor bias and gravity component due to the sensor’s own and tool masses) sensor reading (Jamisola et al., 2005). Generally, due to the displacement (\(d_t\)) between the contact point (t) and the sensor point (s) the torque reading is the sum of the contact torque and moment of the force around the origin of the force sensor frame. Thus, the sensor frame force/torques measurements need be transformed to the task frame as

$$\begin{aligned} \varvec{f}_m = \begin{bmatrix} {}^t\!R_e &{} 0 \\ \left[ \varvec{p}^t_{s}\right] _{\times } {}^t\!R_e &{} {}^t\!R_e \end{bmatrix} \varvec{f}_e, \end{aligned}$$

where \(\varvec{p}^t_{s} \in {\mathbb {R}}^{3}\) is the position vector of the origin of point s with respect to the coordinate frame of t and \({}^t\!R_e\) is the rotation matrix between the end-effector frame and the contact point reference frame.

Based on the forward kinematics of the manipulator, which returns end-effector’s position \(\varvec{p}^b_{e} \in {\mathbb {R}}^{3} \) and rotation matrix \({}^e\!R_b \in {\mathbb {R}}^{3 \times 3}\), the position of the contact point with respect to the base is obtained as

$$\begin{aligned} \varvec{p}^b_{t} = \varvec{p}^b_{e} + {}^e\!R^{\top }_b \varvec{p}^e_{t}, \end{aligned}$$

where \(\varvec{p}^e_{t}\) denotes the position of the contact point t relative to the end-effector. Here, the orientation of the contact point coincidences with the end-effector’s; thus, \({}^t\!R_{b} = {}^e\!R_b\). Hereupon, the rotation matrix of the manipulator (\({}^e\!R_b\)) is to be used for the transformations from the base to the contact frames.

Once the desired task space velocities are obtained, the robot’s Jacobian matrix can be utilized to transform the desired task space velocities, \(\dot{\varvec{x}}_{r}\), into the desired joint space velocities, \(\dot{\varvec{q}}_{r} = \begin{bmatrix} \dot{q}_1,&\ldots ,&\dot{q}_n \end{bmatrix}^{\top }\) as

$$\begin{aligned} \dot{\varvec{q}}_{r} = J^{\dagger }(\varvec{q}) \dot{\varvec{x}}_{r}, \end{aligned}$$
(1)

where \(J^{\dagger } \in {\mathbb {R}}^{n \times 6}\) is the pseudo inverse of the manipulator Jacobian matrix.

The position of the contact point (\(\varvec{p}^g_{t}\)) in the earth-fixed frame (global), on the other hand, based on the vehicle’s position and orientation along the manipulator’s joint configurations is given as

$$\begin{aligned} \varvec{p}^g_{t} = \varvec{p}^g_{v} + {}^g\!R_{v} (\varvec{r}^{v}_b + {}^v\!R_{b} \varvec{p}^{b}_t), \end{aligned}$$

where \(\varvec{p}^g_{t}\) is the position of the vehicle with respect to the global frame, \({}^v\!R_g\) is the rotation matrix between the body-fixed vehicle frame and the earth fixed global frame, the manipulator’s base position with respect to the vehicle’s body fixed frame is denoted with the position vector \(\varvec{r}^{v}_b\), and the matrix \({}^v\!R_b\) is the rotation matrix between the manipulator-base frame and the body-fixed vehicle frame.

2.2 Position-force controllers

The robotic manipulator is required to perform two tasks simultaneously; position control for object manipulation with position disturbance attenuation and force control for establishing a safe and steady contact with the object. The former can be accomplished with a predetermined trajectory, \(\varvec{p}_r\), and a controller minimizing the perturbations at the end-effector introduced by the vehicle’s motion. This controller needs to have fast position tracking performance without excessive overshoots and steady state errors via using noisy sensor measurements that might contain time delays due to the discrepancy between the control and measurement cycles. Thus, we applied a reset controller method with a variable reset band which can be described in the state-space form as

$$\begin{aligned} R: {\left\{ \begin{array}{ll} \dot{\varvec{x}}(t) = A_r \varvec{x}(t) +B_r \varvec{e}_r(t), &{} {\text {if }} (\varvec{e}_r(t), \dot{\varvec{e}}_r(t)) \not \in {\mathcal {B}}_h \\ \varvec{x}(t^+) = A_p \varvec{x}(t), &{} {\text {if }} (\varvec{e}_r(t), \dot{\varvec{e}}_r(t)) \in {\mathcal {B}}_h \\ \varvec{u}(t) = C_r \varvec{x}(t) +D_r \varvec{e}_r(t), \end{array}\right. }, \end{aligned}$$
(2)

where \(\varvec{e}_r\), \(\varvec{x}\), and \(\varvec{u}\) are error, state, and output vectors, respectively (Villaverde et al., 2011). The middle equation represents the reset action applied when the error crosses a threshold, generally zero value. The diagonal matrix \(A_p\) performs reset algorithm on the desired states as \(A_p = \text {diag}\begin{pmatrix} 1-d_r,&\ldots ,&1-d_r,&0,&\ldots ,&0 \end{pmatrix} \) with a reset ratio \(d_r\) which is a constant value with values \(d_r \in \left[ 0, 1\right] \) and allowing us to apply a partial reset (\(d_r\ne 1\)) strategy. The variable reset band can be defined as

$$\begin{aligned} {\mathcal {B}}_h = \left\{ (\varvec{e}(t), \dot{\varvec{e}}(t)) \in {\mathbb {R}}^2 \quad | \quad |h\dot{\varvec{e}}(t) +\varvec{e}(t)| \le \gamma \right\} , \end{aligned}$$
(3)

where h and \(\gamma \) are positive constants and associated with the system’s time delay and band range, respectively (Banos & Barreiro, 2012).

Fig. 3
figure 3

Reset action can reduce the overshot without introducing additional control efforts on the system

Full size image

The advantages of the reset control over a linear compensator can be illustrated with a system consisting of pure integrator and time delay. A numerical example is adapted from Davó et al. (2018), such that a transfer function, \(P(s) = \frac{e^{-0.15s}}{s}\), is considered to be in a feedback loop with a base compensator given by \(\frac{1.4s + 0.3}{s}\) plus a reset band. The step response of the closed loop system with (\(d_r = 0.8\)) and without (\(d_r = 0\)) reset actions while \(h = 0.15\) and \(\gamma = 0.001\) are illustrated in Fig. 3 where the overshoot is eliminated with the reset action. We refer the reader to Banos and Barreiro (2012), Zhao et al. (2019) for more detailed information about how the reset control overcomes limitations of the linear controllers.

The objective of force tracking, on the other hand, requires a force/torque sensor near the contact point, i.e., the end-effector of the robot, so that the interaction force can be measured to generate the desired velocities via an admittance type control algorithm whose motion dynamics in the task space are

$$\begin{aligned} \varvec{f}_{r} - \varvec{f}_m = M_a\dot{\varvec{v}}_f + D_a\varvec{v}_f + K_a(- \varvec{p}_o), \end{aligned}$$
(4)

where \(\varvec{f}_{r}\), \(\varvec{f}_m\), and \(\varvec{v}_f\in {\mathbb {R}}^6\) denote the reference, measured force/torque, the desired end effector velocity vectors, respectively. The object’s position with respect to the end-effector is denoted by \(\varvec{p}_o = \begin{bmatrix} p^x_o&p^y_o&p^z_o&0&0&0\end{bmatrix}^{\top }\). The diagonal matrices, \(M_a\), \(D_a\), and \(K_a \in {\mathbb {R}}^{6\times 6}\) are the admittance controller’s virtual mass (\(m_i\)), damping (\(d_i\)), and stiffness (\(k_i\)) parameters for position (\(i=p\)) and orientation (\(i=o\)). At the contact point, we want the end effector to be perpendicular to the inspected surface at task space. Thereby, the reference force/torque can be denoted as \(\varvec{f}_{r} = \begin{bmatrix} 0&0&f^z_r&0&0&0\end{bmatrix}^{\top }\), which implies that the force controller makes an effort to apply constant force, \(f^z_r\), in z direction and zero torque in x and y directions to prevent misalignment at the tool frame. To eliminate force components affecting in other directions, calculated \(\varvec{v}_f\) will be multiplied with a task matrix \(\Omega = \text {diag}\begin{pmatrix} 0,&0,&1,&1,&1,&0 \end{pmatrix} \) before using in the manipulator control algorithm as in Moura et al. (2018), Cetin et al. (2021).

Fig. 4
figure 4

Block diagram representation of the implemented position-force control algorithm for the manipulator interacting with the environment

Full size image

The vehicle motion affects the contact point position. To phase out this, similar to the chicken head stabilization, the measured base motion distortion (\(\varvec{p}_v\)) is embedded into the reset controller’s reference trajectory. In this way, the environmental disturbances affecting the main body of the overall system would be eliminated at the contact point.

One can utilize Jacobian matrix as in (1) for transforming task space velocities into the joint space velocities but, the motion might be unstable near singular joint configurations. Thereby singularity handling algorithms need to be implemented for safe operations (Jamisola et al., 2005). Here, following Scherzinger et al. (2017) and Lee et al. (2020), to solve the inverse kinematics problem we implement a forward dynamic control (FDC) method. The designed control architecture is illustrated in Fig. 4 where the Jacobian transpose is utilized, rather than Jacobian inverse, and nominal robot model, \(P_m\), generates the desired joint velocities (Scherzinger, Roennau, & Dillmann, 2019). The control input, \(\varvec{\tau }_c\), within the FDC method is generated via an impedance controller which is denoted as a proportional-derivative (PD) controller for non-redundant robots as

$$\begin{aligned}&M_m(\varvec{q})\ddot{\varvec{q}} + \varvec{c}_m (\varvec{q},\dot{\varvec{q}}) + \varvec{g}_m (\varvec{q}) = \varvec{\tau }_c,\nonumber \\&\varvec{\tau }_c = J^{\top }\varvec{f}_c + G_m (\varvec{q}) - B\dot{\varvec{q}}\, \text { and } \,\varvec{f}_c = K\varvec{e}+D\dot{\varvec{e}}, \end{aligned}$$
(5)

where \(M_m \in {\mathbb {R}}^{6\times 6}\) denotes the manipulator’s positive definite inertia matrix, \(\varvec{c}_m\) and \(\varvec{g}_m \in {\mathbb {R}}^{6} \) represent Coriolis an centrifugal terms and gravitational components, respectively. The task space pose error is denoted as \(\varvec{e} \in {\mathbb {R}}^6\). The diagonal matrices, \(K = k_i\text {I}\), \(D=d_i\text {I}\in {\mathbb {R}}^{6\times 6}\) are the impedance controller’s gains for position (\(i=p\)) and orientation (\(i=o\)). Additional joint space damping, \(B =b\mathrm {I} \in {\mathbb {R}}^{6\times 6}\), is injected to dissipate the energy generated near singular configurations as the task space inertia and Coriolis increase near singular configurations (Lee et al., 2020). On the other hand, to eliminate the effect of the injected damping at a nonsingular motions, b is modified with a direct-delta function as

$$\begin{aligned} b = b_{max}\text {exp}\left( -\frac{\sigma _{min}^2}{a^2}\right) , \end{aligned}$$
(6)

where a is a positive constant determining the width of the direct-delta function, see Fig. 5, \(b_{max}\) is the maximum damping constant, and \(\sigma _{min}\) is the minimum singular value of the Jacobian matrix measuring the distance from the singular joint configuration.

Fig. 5
figure 5

Determining the injected damping via a direct-delta function (\(b_{max} = 1.25\,\mathrm{N}\,\mathrm{s}\,\mathrm{m}^{-1}\))

Full size image

2.3 Directional manipulability and constrained PSO

It is a common approach to use a measure of dexterity in studies that aim at optimizing the base location of serial manipulators. The manipulability index, \(\mu \), is employed as a proxy to measure the dexterity associated with a feasible configuration of the manipulator. It can be derived for non-redundant manipulators as

$$\begin{aligned} \mu = \sqrt{det(J(\varvec{q})J(\varvec{q})^{\top })} =\prod _i \sigma _i, \end{aligned}$$
(7)

where \(\sigma _i\) denotes the singular values for the Jacobian matrix (Yoshikawa, 1985). One can use the dexterity measure to define an objective function for the optimum base localisation. In a near-singular configuration, the determinant of the Jacobian of a serial manipulator almost diminishes and hence \(\mu \) in (7) approaches to zero. Maximizing this index as an optimization criterion maximizes the transformation from joint velocities to task-space velocities.

At task space, to enhance not only the effecting velocities but also the force transmission, the duality between the velocity and force control directions needs to be addressed (Chiu, 1988). Using the dexterity measure, considering the duality criterion, and counting the task space control directions we define a directional optimization index as

$$\begin{aligned} o_{i}&= {}^f\!w\left[ {}^f\!\varvec{u}^{\top } (J_i(\varvec{q})J_i(\varvec{q})^{\top }){}^f \!\varvec{u}\right] \nonumber \\&\quad + {}^v\!w\left[ {}^v\!\varvec{u}^{\top } (J_i(\varvec{q})J_i(\varvec{q})^{\top })^{-1} {}^v\!\varvec{u}\right] , \end{aligned}$$
(8)

where \({}^f\!w\) and \({}^v\!w\) are positive constants representing the relative importance of the force and velocity controllers, respectively, and the directional vectors can be defined as \({}^f\!\varvec{u} = {}^c\!{\mathcal {R}}^{\top }_b \begin{bmatrix} 0&0&1&1&1&0 \end{bmatrix}^{\top }\) and \({}^v\!\varvec{u} ={}^c\!{\mathcal {R}}^{\top }_b \begin{bmatrix} 1&1&0&0&0&1\end{bmatrix}^{\top }\) with \({}^c\!{\mathcal {R}}_b =\text {diag}({}^c\!R_b, {}^c\!R_b)\) matrix which is the diagonal combination of the rotation matrix, \({}^c\!R_b\), between the manipulator-base frame and the contact point. One can define a cost function which is minimised as a function of the total optimization index along a predetermined task trajectory as

$$\begin{aligned} f_{o}(o) = \frac{1}{\sum _{i=0}^{n}o_i}, \end{aligned}$$
(9)

where \(i= [0 \ldots n]\) stands for the points on the task trajectory. Minimizing this cost function corresponds to maximizing the total optimization index and in turn maximizing the effective force transmission in the selected directions and enhancing the velocity control in others. The two weighting factors, \({}^f\!w\) and \({}^v\!w\), represent the relative importance of the force and velocity controllers in orthogonal directions within the optimization index and allow for managing the trade-off between maximizing the force in the selected directions and having a fine velocity control in the other directions. These are user defined parameters according to whether maneuverability in the direction of force transmission or maneuverability in the direction of velocity control are prioritized and to what degree.

PSO is a bio-inspired population based heuristic optimization technique (Kennedy & Eberhart, 1995). The idea is intuitive; a group of swarm/particles search for an optimum location (i.e. cornfield) altogether and each member has the knowledge of their current local optimum (particle best position \(p_{pbest}\)) and global optimum (group best position \(p_{gbest}\)) locations during the search. Each particle, k, is associated with two vectors, the velocity vector \(V_k =\begin{bmatrix}v^1_k,&v^2_k,&\ldots ,&v^d_k \end{bmatrix}\) and position vector \(P_k = \begin{bmatrix}p^1_k,&p^2_k,&\ldots ,&p^d_k \end{bmatrix}\), where d stands for the dimension of the search space. The velocity and position of each particle are initialized with random vectors within the feasible ranges for each parameter. The optimizer (evolutionary process) adjusts the velocity and position of the particle k on dimension d as

$$\begin{aligned} v^d_k&= \omega v^d_{k-1} + \epsilon _1 r^d_1 (p^d_{pbest}-p^d_k) + \epsilon _2 r^d_2 (p^d_{gbest}-p^d_k), \nonumber \\ p^d_k&= p^d_{k-1} + v^d_k, \end{aligned}$$
(10)

where \(p_k\) and \(v_k\) denote the particle’s position and velocity, respectively, \(r^d_1\) and \(r^d_2\) are two uniformly distributed random numbers independently generated in [0, 1]. The particles are evaluated based on their new position scores; \(p_{pbest}\) and \(p_{gbest}\) are updated at every iteration. The search process is repeated until a pre-determined stopping criterion is met; e.g., the search is repeated for a fixed number of times (\(l_{max}\)) or a defined cost function value is achieved. The effect of the particle’s inertia, cognitive and social behaviours are denoted as \(\omega \), \(\epsilon _{1}\), and \(\epsilon _{2}\), respectively, see for instance (Eberhart & Kennedy, 1995; Shi & Eberhart, 1998; Zhan et al., 2009) for more information about the effect of each variable and how to tune them. One can implement this algorithm to find an optimum position for the vehicle such that the cost function in (9) is reduced to maximize the manipulability of the manipulator mounted on it.

figure f

When the mobile manipulator is in the vicinity of an object (such a a valve) which the robotic manipulator can and is desired to make a contact with (within its workspace), safety becomes the main concern as collision may occur between the vehicle and the environment where the object is located (such as a pipe on which the valve is located). In order to prevent collision, a safety workspace restricting the potential movement of the vehicle can be defined. Particles are then initialized in that workspace and the search space is constrained within the PSO algorithm. In this study, we illustrate this concept by using a 3D sphere to restrict the motion of the vehicle once the contact is established as

$$\begin{aligned} g(p) = (p^x - p_0^x)^2 + (p^y - p_0^y)^2 + (p^z - p_0^z)^2 -r^2 \le 0, \end{aligned}$$
(11)

where r denotes the vehicle’s operational workspace radius allowed during the interaction, \(p_0^i\) is the position of the vehicle when the first contact is established, and \(p^i\) is the relative position of the vehicle with respect to \(p_0^i \in {\mathbb {R}}^3\). Also, \(v_k\) is restricted to the range \(\left[ -v_{max}, v_{max}\right] \) to prevent particles from converging outside of the search space (Corne et al., 1999). Accordingly, the constrained optimization problem to find a location for the vehicle can be described as

  • Find \(p^x\), \(p^y\), \(p^z\) to minimize \(f_{o}\),

  • Subject to \(g(p) \le 0\).

The original PSO design lacks a mechanism to handle the constraints in an effective way. Most of the constrained PSO designs adopt the popular constraint handling techniques that are built for evolutionary algorithms (Runarsson & Yao, 2005; Takahama & Sakai, 2006). Different methodologies have been implemented regarding the constraint handling with PSO including penalty methods (Parsopoulos & Vrahatis, 2002), comparison criteria or feasibility tournament (Coello, Pulido, & Lechuga, 2004; He & Wang, 2007), and Lagrange-based method (Krohling & Dos Santos Coelho, 2006). Here, a feasible region constraint handling algorithm and a penalty method are implemented; initialization is made within the feasible search space such that constraint (11) is satisfied and the particles only memorize their and the group’s best feasible solutions located in the search space (Isiet & Gadala, 2019; Xiaohui & Eberhar, 2002). The utilized constrained PSO algorithm solves the cost function as

$$\begin{aligned} F_{o} = {\left\{ \begin{array}{ll} f_{o},&{} {\text {if feasible}},\\ f_{o} + \beta _1\sum \limits _{i=1}^{d}s + \beta _2\sum \limits _{i=1}^{d}y,&{} {\text {if infeasible}}, \end{array}\right. } \end{aligned}$$
(12)

where \(F_{o}\) and \(f_{o}\) are the penalty and original functions, \(\sum _{i=1}^{d}s\) is the sum of d violated constraints, \(\sum _{i=1}^{d}y\) is sum of the amount of d violated constraints, and \(\beta _1\) and \(\beta _2\) are the penalty factors. See Algorithm 1 for the depicted constrained search method.

2.4 Case study: valve turning

Valve manipulation (e.g., Di Lillo et al., 2020) similar to a wheel rotating under environmental disturbances is adopted as a case study in the validations. Namely, a gripper attached to a robotic arm mounted on a moving platform grasps a valve from its outer circle (or from its extreme if it is a T-shape valve) such that the end-effector’s z-coordinate frame is perpendicular to the valve’s rotation plane. In this configuration, our control direction in the task frame would be defined as; x, y, and yaw for velocity control and z, roll, and pitch for force/torque control. In this way, valve manipulation can be carried out with the velocity control and the orientation between the gripper and the valve itself will be maintained with the force/torque control.

An imitation of underwater manipulation was chosen as a case study where the manipulator motion and environmental interaction force affect the motion of the base vehicle. These effects are generated via modelling for the evaluation of the performance of the designed controller. The general equation of motion of an underwater vehicle can be written as

$$\begin{aligned} M_r\dot{\varvec{\nu }} +C_r(\varvec{\nu }) \varvec{\nu } + D_r(\varvec{\nu })\varvec{\nu } +\varvec{g}_r(\varvec{\upeta }) = \varvec{\tau }_r + \varvec{\tau }_d, \end{aligned}$$
(13)

where \(\varvec{\nu }\) is the body fixed linear (surge, sway, and heave) and angular (roll, pitch, and yaw) velocity vector, \(\varvec{\upeta }\) is the earth fixed position and attitude vector. The vehicle’s positive definite inertia matrix including the added inertia, Coriolis and centripetal terms, and hydrodynamic lift and damping matrices are denoted by \(M_r\), \(C_r\), and \(D_r \in {\mathbb {R}}^{6\times 6}\), respectively. The vectors \(\varvec{g}_r\), \(\varvec{\tau }_r\), and \(\varvec{\tau }_d \in {\mathbb {R}}^{6}\) denote gravitational components and buoyancy forces, propulsion forces, and induced disturbance due to the manipulator motions (Fossen, 2011). The induced disturbances due to the manipulator motions, \(\varvec{\tau }_d =[\varvec{\tau }_f^{\top },\varvec{\tau }_m^{\top }]^{\top }\), can be calculated as

$$\begin{aligned} \varvec{\tau }_d = \left[ \begin{array}{cc} {}^v\!R_b &{} 0 \\ \left[ \varvec{r}^v_b\right] _{\times }{}^v\!R_b &{} {}^v\!R_b \end{array}\right] \varvec{f}_b, \end{aligned}$$
(14)

where \(\varvec{f}_b\) represents the forces and moments acting on the base frame of the manipulator.

3 Experimental validation

3.1 Experimental setup

The experimental setup consists of a parallel manipulator known as a Stewart platform, a Universal Robot (UR3), a Robotiq 2-finger gripper (2F-140), an Intel RealSense camera (D455), and an ATI Gamma force/torque sensor (with ATI Force/Torque 9105-NETB sensor box). The camera and UR3 robot are mounted on the Stewart platform and the force/torque sensor attached to the robot’s end effector via a mechanical adaptor and the gripper is attached to the end-of the sensor. Thus, the setup enables the robot to interact with the environment (in this case a PVC valve with diameter equal to 0.1 m), see Fig. 6. The platform and UR3 robot each have six degrees of freedom and each controlled by their own control boxes providing 33 Hz and 125 Hz control cycles, respectively.

Fig. 6
figure 6

Experimental setup consisting of a Stewart platform, UR3 robotic arm, force/torque sensor, 2-finger gripper, and a camera. A rigid wheel type valve was used as an object that the robot arm is interacting with and manipulating

Full size image

The explained swarm algorithm is used to find the optimum positions for the platform and waypoints to generate a smooth trajectory using a quartic spline. An April-tag marker is used to locate the valve and also to measure any perturbation effecting the platform (Wang & Olson, 2016). To create a stable interaction with the environment, the FDC method, in (5) with (6), along with the reset control, in (2) with (3), and admittance control, in (4), architectures were implemented when an object enters into the workspace of the manipulator, by using the force/torque sensor’s measurements with a sampling frequency (\(f_s\)) equivalent to the robot’s control cycle (\(f_s=125\,\mathrm{Hz}\)). The Stewart platform, robotic manipulator, and gripper are all controlled via the ROS middleware for ease of integration by a computer with a 3.20 GHz CPU. This experimental setup imitates a mobile manipulator systems that could be used for subsea or aerial applications.

3.2 Results

Initially, to evaluate the controller actions on the chicken-head problem, the base of the manipulator was moved in z-direction \(\pm 0.18\,\mathrm{m}\) as a perturbation (Experiment 1), with the controller parameters given in Table 1. Thus, the performance of the reset action was observed with the default proportional-integral (PI) controller. The measured base motion by means of the fiducial marker was used within the controller to eliminate the undesired move at the contact point. The evaluation is based on the Euclidean norm of the recorded position errors with respect to the reference point computed as

$$\begin{aligned} \Vert E_{cont.}\Vert = \sqrt{\sum _{i = x}^{l}\sum _{j=1}^{n}e_{ij}^2}, \end{aligned}$$

where the controlled axes are denoted by \(l = x, y, z\) and n denotes the data size. For a \(t_{max}\) seconds experiment, with data registration at a rate \(f_s\), the amount of samples recorded will be \(n = t_{max}f_s\). Figure 7 shows that despite the fast base motion, \(|v_z| = 0.18\,\mathrm{m}\,\mathrm{s}^{-1}\), the controller could relatively keep the end-effector position stationary with respect to the global frame with a maximum error equivalent to \(1\,\mathrm{mm}\) and the reset action can reduce the overall position tracking error by approximately 27% when compared to the base controller.

Table 1 Controller and optimization parameters
Full size table
Fig. 7
figure 7

End-effector position, position error (E), and integral error term within the controller when base of the manipulator is moved \(\pm 0.18\,\mathrm{m}\) in the z-direction. Reset action eliminates the overshoot and reduce overall tracking error via resetting the integral action (undermost figure); \(\Vert E_{PI}\Vert = 0.212\) and \(\Vert E_{PI+Reset}\Vert = 0.154\)

Full size image

Then, a simple experiment (Experiment 2) was executed via using a pendulum with a mass of 8 kg and \(l=0.26\,\mathrm{m}\) to compare the torque measures generated by the manipulator to accomplish the same task in different base positions. The robot was required to hold a pendulum 0.05 m away from the equilibrium position which, creates restoring forces on the contact point due to the gravity.

A threshold force, \(f_{th}\), was specified as 2 N which allowed us to determine whether or not a contact is established with the pendulum. Once the end-effector has contact with the pendulum, the manipulator moves the mass to the desired position in 5 s and holds there around another 5 s. The task was repeated with a non-optimumFootnote 1 (practically the nominal position of the platform; \(1.196\,\mathrm{m}\) above and parallel to the ground) and with an optimum postures where, in the later case, the base was located at the optimum position after establishing contact with the pendulum. The last 2 s of the joints’ torques measurements were is used in a comparison index calculated as

$$\begin{aligned} \tilde{\tau } = \left\| \frac{\sum _{i}^{n}\tau _{mr}(j,i)^{\top }}{n}\right\| , \quad j \in [1,6], \end{aligned}$$

where \(\tau _{mr}\) denotes the stored joints’ effort measurement as a matrix. A lower value of this index is desired for the same task.

Fig. 8
figure 8

Left holding a pendulum with 8 kg mass at a position approximately 0.05 m from the equilibrium position. Right repeating the same task while the base is located an optimum posture such that force/torque transmission property of the manipulator, from joint to end-effector space, is enhanced

Full size image
Fig. 9
figure 9

Force measurements in z direction at task space during two experiments; non-optimum and optimum postures. Threshold force was determined as \(f_{th} = 2\,\mathrm{N}\) to determine the contact

Full size image

Figure 8 shows different postures of the manipulator during the experiments. Figure 9 shows the end-effector interaction force in z direction at task space for both cases, which verifies that a very similar level of task effort (slightly more effort in case of optimal location) was performed by the robot with both the non-optimal and optimal base locations. Table 2 gives the joint torque effort computations for both robot configurations when the robot was pushing the pendulum and when the robot was in free space without pushing the pendulum. Moving the base location to a desired position reduces the required joints’ efforts around 9.3% for the same task: in the non-optimum case \( \tilde{\tau }_{nopt} = 2.1974\) and in the optimum posture \( \tilde{\tau }_{opt} = 1.99\). This is achieved despite the fact that slightly more force is applied to the end-effector while the robot was in the optimum posture, as seen in Fig. 9.

Table 2 Joints’ efforts in different postures and contact conditions. First Row: Less joint-torques are required in optimum posture compared to the non-optimum while holding a pendulum. Second Row: More effort is required for the gravity compensation in the optimum posture compared to the non-optimum, indicating that the less torque requirement in the First Row with the optimum posture, is obtained despite larger gravity compensation compared to the non-optimum posture
Full size table

In a given posture, the total joint torques are determined by the gravitational effect (the torques required to hold the robot at that joint configuration) and the force applied to the end effector (or any external disturbances effecting the system). To analyse the effect of the gravitational term in the effort calculations, we moved the arm to the previously obtained joint configuration in free space (without the pendulum, no force is applied at end-effector) and joints efforts are analysed in these configurations. It was observed that gravitational term in optimal posture is slight higher than the non-optimal posture, around 4.9%: in the free space non-optimum case \(\tilde{\tau }_{nopt} = 2.1661\) and in the optimum posture \(\tilde{\tau }_{opt} = 2.2715\) (Table 2). Despite this, the optimal posture overall requires less effort to apply the same level of force at the end effector. This verifies that the difference of 9.3% effort with the optimal base location is indeed due to optimally locating the base for maximum effort transmission.

Table 3 Overall directional manipulability of the robotic arm mounted on the platform through the valve turning
Full size table

Finally, a valve turning task at optimum base location was executed with the following experimental protocol (Experiment 3);

  1. 1.

    Manipulator grasps the valve under base disturbances,

  2. 2.

    Optimum base position is estimated,

  3. 3.

    Platform is moved to the optimum location,

  4. 4.

    Valve is manipulated with a pre-defined trajectory (\(0-\pi /2\,\mathrm{rad}\)).

Different experimental scenarios were carried out for comparisons. First, a benchmark performance index was obtained via manipulating the valve with a fixed base location (T0) (platform is at its nominal position). Then, another experiment (T1) was carried out assuming that the robotic arm was mounted on a ground based vehicle which had omnidirectional motion capabilities which could translate the manipulator base location in two directions. Thus, an optimum base location was estimated in x- and y-directions (2D) and quartic splines were implemented to generate a smooth path for the vehicle in between the initial and optimal locations.

As a final effort, it was assumed that there existed an underwater vehicle manipulator system (UVMS) such that the vehicle could be controlled to move in x-, y-, and z-directions (3D). Optimal base locations were estimated for these directions and an AUV model along with its controller were implemented to generate the motions of the vehicle. The experiment repeated while assuming that there was no (T2) and there existed (T3) environmental disturbances affecting the vehicle motion in all three directions. The following position disturbance adopted from Dunnigan and Wronka (2011) was applied to evaluate the performance of the task execution controller

$$\begin{aligned} p_i(t) = Asin\left( \frac{2\pi t}{T_i} + \psi _i \right) \quad \text {for} \quad i \in \left\{ x, y, z\right\} , \end{aligned}$$

where \(A = 0.02\,\mathrm{m}\) denotes the amplitude, \(T_i\) is the period (\(T_x= 3\,\mathrm{s}\), \(T_y= 2.25\,\mathrm{s}\), \(T_z=4.5\,\mathrm{s}\)), and \(\psi _i\) is the phase (\(\psi _x=\pi /2\,\mathrm{rad}\), \(\psi _y= 0\,\mathrm{rad}\), \(\psi _z=\pi \,\mathrm{rad}\)). It must be stated that, the disturbance was unknown to the position controller and a camera in conjunction with the fiducial marker was utilized to measure the deviation from the desired path.

Fig. 10
figure 10

The platform performs the motion of a hypothetical underwater vehicle under disturbances

Full size image
Fig. 11
figure 11

Estimated optimum points (within T1, T2, and T3) and measured platform position in the last experiment within the constrained motion space

Full size image

The performance index of the manipulator, as motivated, was enhanced at the optimum base locations, see the obtained total optimization index (\( o_{total} = \sum _{i=0}^{n}o_i\)) within the aforementioned experiments in Table 3. In 2D optimization the overall directional manipulability was increased 75% and in 3D localization it is boosted approximately 165% compared to the fixed base position. Perturbations, naturally, reduce the overall performance as the base would be displaced from the desired position, see Fig. 10 for the base motion, optimum location, and deviation from the path due to the disturbances during T3. Figure 11 illustrates the base motion in the last experiment (T3) and the estimated optimum positions for all the experiments within the constrained search space.

Fig. 12
figure 12

Force and torque measurement during the interaction where S and F denotes the start and finish points of the task execution. Bottom graph: position error at the contact point caused by the undesired base motions

Full size image

For a safe operation, not exceeding the maximum force/torque values applicable to the manipulator and the manipulated object is essential along with adequately attenuating any undesired base motion at the contact point. The interaction force and torque values during the last experiment (T3) are illustrated in Fig. 12, where the force and torque values caused by the interaction are not exceeding 2 N and 0.2 N m, respectively, with the help of the implemented force/torque controller. Also, the reset control architecture sufficiently attenuates any base motion effect at the contact point such that \(\max (|E|)= 0.005\,\hbox {m}\), which can be compensated with the compliance mechanism of the gripper.

To observe the effects of the interaction forces on the vehicle, the dynamics of an AUV, the Falcon underwater vehicle was emulated using the Stewart platform, via implementing (13) and (14) with the dynamic parameters adopted from Berg (2012) and Manhães et al. (2016). The simulated AUV position is transferred to the platform in addition to the artificially generated disturbances and the computed optimum location as a reference trajectory. One can state that the proposed approach is applicable to such underwater vehicles with strong controllers as the deviation of the base vehicle from the reference position is almost negligible and the implementation of the force/torque and the reset controllers leads to the limited interaction forces. Parameters used in the control architecture and the optimization algorithm (Bonyadi & Michalewicz, 2017) are given in Table 1. We refer the reader to Scherzinger et al. (2019) and Tugal et al. (2018) for more detailed information about the impact of the manipulator’s model and impedance controller used in FDC approach and admittance controller parameters on the system’s behaviour, respectively.

4 Conclusion

This study validates the feasibility of the directional manipulability to augment the force/torque transmission and velocity control from joint space to the task space for a mobile robotic manipulator system. Constrained particle swarm optimization was implemented to find an optimal position for the vehicle. To attenuate the undesired motion at the contact point along with avoiding excessive motions near the singular joint configurations and to establish a stable interaction with an unknown object; reset, forward dynamic, and impedance controllers were developed for the robotic arm. In this approach we combine a metaheuristic optimisation technique and the directional optimization index with the control architecture and in this way we both enhance manipulability and ensure stability while interacting. This approach can be used with any vehicle-manipulator system or stationary robotic systems.

The proposed optimization and control algorithms were evaluated with an experimental setup allowing to emulate mobile robotic manipulator dynamics within a limited motion space. The results show that a wheel type valve, typically used in underwater assets, can be manipulated at an optimum vehicle position where force transmission and fine velocity control properties of the robotic arm mounted on it are enhanced. Also, the arm can apply executable force/torque in the rotation direction of the valve in a safe manner despite moderate disturbances affecting the system.