Kinematic Modeling and Motion Planning of the Mobile Manipulator Agri.Q for Precision Agriculture

Abstract

In recent years, the study of robotic systems for agriculture, a modern research field often shortened as “precision agriculture”, has become highly relevant, especially for those repetitive actions that can be automated thanks to innovative robotic solutions. This paper presents the kinematic model and a motion planning pipeline for a mobile manipulator specifically designed for precision agriculture applications, such as crop sampling and monitoring, formed by a novel articulated mobile base and a commercial collaborative manipulator with seven degrees of freedom. Starting from the models of the two subsystems, characterized by an adjustable position and orientation of the manipulator with respect to the mobile base, the linear mapping that describes the differential kinematics of the whole custom system is expressed as a function of the input commands. To perform pick–and–place tasks, a motion planning algorithm, based on the manipulator manipulability index mapping and a closed form inverse kinematics solution is presented. The motion of the system is based on the decoupling of the base and the arm mobility, and the paper discusses how the base can be properly used for manipulator positioning purposes. The closed form inverse kinematics solution is also provided as an open-source Matlab code.

1. Introduction

The development of mobile manipulators, defined as complex robotic systems composed by a mobile base and a robotic manipulator mounted upon it, has become a relevant research field inside the service robotics world. They were first invented at the end of 1990s for industrial applications, and with the development of the so-called Autonomous Industrial Mobile Manipulators (AIMM) [1], their relevance is strictly related to the evolution of industrial production processes, currently characterized by the introduction of digital technologies, e.g., the integration of autonomous robotic systems into the traditional production plants for massive and repetitive tasks. To meet the current demand of product customization and differentiation, researchers and manufacturers from all the world have created several AIMM solutions, that integrate autonomous mobility, exteroceptive and proprioceptive sensing, and dexterous manipulation [2,3,4]. Such systems can autonomously navigate inside a production plant, perform several tasks, e.g., packaging, painting, and logistics, and can be easily set up and programmed to change their main purpose.

Moreover, service robotics can take advantage of such complex systems and their related functionality. Indeed, recent years have seen the birth of mobile manipulator solutions for non-industrial objectives, e.g., assistance, precision agriculture, and search and rescue, as briefly depicted in Figure 1.

In 2021, Kemp et al. presented Stretch [5], specifically designed for indoor human environments and telepresence. The key design goal was the reduction in the number of actuators, and it ended up with a conventional differential drive mobile base and a manipulator with three degrees of freedom (d.o.f.). For motion planning purposes, the authors considered two separate operation modes, the navigation and manipulation mode, underlining how the correct position of the base is fundamental for a correct task execution. Again for assistive and collaborative applications, since 2006, researchers from Toyota company have developed HSR (Human Support Robot) [8], a domestic mobile manipulator with a dual wheel caster mechanism and a seven d.o.f. robotic arm. To pick objects from the floor or tables, the first manipulator d.o.f. enables the motion along the vertical axis of the structure. Focusing on humanoid systems, Li et al. [9] implemented an off-the-shelf dual armed humanoid manipulator and an omnidirectional base to create TRINA (Tele-Robotic Intelligent Nursing Assistant) a complex system with 26 d.o.f. commanded by an operator console for telepresence in hospitals.

The development and implementation of such systems can be effective also in the agriculture field, where all the tasks have been traditionally carried out by manual labor. In 2017, Silwal et al. [16] presented an autonomous mobile apple harvester formed by a six d.o.f. arm mounted upon a generic mobile base, successfully tested in a commercial apple orchard. Even though the system was incredibly innovative, there was no cooperation between the mobile base and manipulator motion, since the mobile base is fixed during the apple harvesting. A similar work was presented by De Preter et al. in 2018 [12], where a commercial robot from Octinion company was used for strawberry harvesting. The integration of mobile manipulators in the agriculture field can be useful also for selective spraying, thus reducing the production cost and the use of pesticides on crops. To this end, in 2016 Oberti et al. [11] presented a novel integration of a selective spaying system on the modular agriculture robot CROPS, with a multispectral imaging system and an associated image recognition to select the diseased target area.

In all the cited precision agriculture cases, the development of the mobile manipulator systems was substantially done by the implementation of a custom manipulator and/or sensing system upon a generic and multi-purpose mobile base, and the mounting position of the arm with respect to the base was fixed. This paper presents the kinematic model and a simplified motion planning method for the novel Agri.Q mobile manipulator, designed by the researchers from Politecnico di Torino for precision agriculture applications, where the robotic arm mounting position and orientation with respect to the mobile base is adjustable thanks to the mobile base mobility characteristics.

The main contributions to the subject investigated in this work are:

-

The differential kinematic model of the whole custom system, described by a linear mapping from the velocity input commands to the system velocities. The kinematic model for the planar motion of the base was already completed by the authors in [17], where the base was treated as a mobile rover and the pitch mobility was not considered; so, the work is here significantly extended considering the pitch motion, which translates and rotates the manipulator base, and the manipulator mobility itself.

-

The description of a decoupled motion planning algorithm for sampling and pick/place tasks, where the base mobility is used to properly reach the target and also take advantage of the manipulator dexterity.

-

Manipulator inverse kinematics formulation with the use of the elbow or swivel angle, which is a closed form analytic solution of the inverse kinematics problem. An open-source algorithm written in Matlab code is provided (https://github.com/giocolucci/Jaco2SwivelIK; https://it.mathworks.com/matlabcentral/fileexchange/108419-jaco2swivelik, accessed in 21 March 2022).

The next sections will be organized as follows. First, a brief description of the Agri.Q system is provided in Section 1.1. Next, Section 2 focuses on the kinematic model, and Section 3 focuses on the simplified motion planning pipeline.

1.1. Agri.Q Mobile Manipulator

The Agri.Q prototype was formed by a custom mobile base [18], specifically designed for precision agriculture applications, and a commercial seven d.o.f. collaborative arm Kinova Jaco2. The mobile base belongs to the articulated type, with two modules, a front module and a back module; each of these is provided by two locomotion units, one for each side, formed by two traction wheels. Each locomotion unit is commanded by a single electric motor, and a chain drive provides the mechanical power to the two wheels, which are connected to the main module with a rocker with a passive revolute joint, to enhance the obstacle overcoming abilities (Figure 2). Since the base has to navigate inside loose and irregular soil, the use of eight wheels ensures a wide contact surface between the vehicle and the ground, as occurs with a track system. Moreover, the traction control can decide to move the system using a front-wheel drive or all-wheel drive, enabling navigation inside quite steep rows of vines. The base is also provided with a solar panel, designed for drone landing and the recharge of the 24 V lithium-ion battery that feeds the required electric power to the traction elements, the robotic arm, and to the electronic components. Thanks to a pitch mechanism [19], the orientation of the panel is adjustable in order to to take full advantage of the recharging phase in different sunlight conditions [20].

The robotic arm was the Jaco2 model from Kinova company, a collaborative serial manipulator with seven d.o.f. designed for assistance and other collaborative tasks. Since the d.o.f. of the arm is larger than the dimension of its workspace, it is defined as intrinsically redundant, which makes it able to perform difficult tasks and use the kinematic redundancy to avoid obstacles or reach a target inside a narrow space. To take advantage of the base mobility, the manipulator was fixed to the solar panel, so the pitch motion of the base also results in a modification of the mounting position and orientation of the arm with respect to the mobile base, as illustrated in Figure 2.

2. Kinematic Model

2.1. Hypotheses and Representation of the System

To study the 2-D motion of the articulated mobile base, the rover platform was modeled as two modules, a front module and back module, connected by a revolute joint centered in the origin of $\{F\}$, that enabled the yaw motion in the $\{{\widehat{i}}_O,{\widehat{j}}_O\}$ plane, as described in Figure 3a. The traction power was provided by the front wheels, that were commanded by ${\omega }_L$ and ${\omega }_R$, the angular velocity of the left front wheels and angular velocity of the right front wheels, respectively. It is worth pointing out that the angular velocity was the same for the two right or left wheels, since each locomotion unit is commanded by a single motor and the chain drive does not cause velocity disparities.

The following assumptions were made:

-

The traction wheels are subject to pure rolling conditions;

-

No lateral slip of the front and back modules is enabled;

-

The surface is flat and no out-of-plane motion are considered;

-

Each couple of wheels can be reduced to a single virtual wheel with the rotation axis aligned with the module body, as showed in Figure 3b.

The latter assumption, that treats the front module as a differential drive system, was already presented and discussed by the authors in [17].

The mobile base was also provided with a pitch mechanism [19], that can translate and rotate $\{R\}$, the robotic arm base reference frame, in the $\{{\widehat{i}}_B,{\widehat{k}}_B\}$ plane, i.e., it can translate and rotate the serial manipulator, as shown in Figure 4, by the angular velocity command $\dot{\gamma }$.

2.2. Kinematic Model of the Mobile Base

The number of degrees of freedom (dof) of the system, composed by the front module, back module, and pitch system, is equal to five, that corresponsd to the four dof in $\{{\widehat{i}}_O,{\widehat{j}}_O\}$ plane and the pitch motion in $\{{\widehat{i}}_B,{\widehat{k}}_B\}$. The chosen set of generalized coordinates of the system is the following:

$$\underline{q_P}=\{x_1,y_1,{\varphi }_1,\delta ,\gamma \},$$

(1)

where $x_1$ and $y_1$ are the Cartesian coordinates of the origin of $\{F\}$ with respect to $\{O\}$, ${\varphi }_1$ is the yaw angle of $\{F\}$ with respect to $\{O\}$, $\delta$ is the relative yaw between front and back module defined as:

$$\delta ={\varphi }_1-{\varphi }_2,$$

(2)

and $\gamma$ is the pitch angle of the panel.

The relationship between the command vector $\underline{u}$ and $\underline{q}$ can be found in the form:

$$\underline{{\dot{q}}_P}=G\left(\underline{q_P}\right)\underline{u_P},$$

(3)

where $\underline{u}=\{{\omega }_L,{\omega }_R,\dot{\gamma }\}$, as explained before, and the 5 × 3 $Author:Thechangeiscorrect$ matrix $G\left(\underline{q_P}\right)$ expresses the linear mapping between the time derivative of the generalized coordinates of the system and the system input $\underline{u}$. For this, the Cartesian coordinates $\{x_2,y_2\}$ of the origin of $\{B\}$, which describe the Cartesian coordinates of the back module of the system, can be written as a function of $\{x_1,y_1\}$:

$$x_2=x_1-bcos\left({\varphi }_2\right)$$

(4)

$$y_2=y_1-bsin\left({\varphi }_2\right)$$

(5)

Moreover, a non-holonomic equation constraint for the back module can be introduced, which expresses the absence of lateral slip of the module:

$$\underline{v_2}·\widehat{j_B}=0$$

(6)

where $\underline{v_2}=\{{\dot{x}}_2,{\dot{y}}_2\}$ is the velocity of the origin of $\{B\}$ expressed with respect to $\{O\}$, and $\widehat{j_B}=\{-sin{\varphi }_2,cos{\varphi }_2\}$ is the unit vector of the reference frame $\{B\}=\{{\widehat{i}}_B,{\widehat{j}}_B,{\widehat{k}}_B\}$.

By deriving Equations (4) and (5) with respect to time and replacing them into (6), the following result can be obtained:

$$\dot{\delta }={\dot{\varphi }}_1-\frac{sin\left(\delta \right)}{b}|\underline{v_1|}.$$

(7)

Thus, the linear mapping between $\{|\underline{v_1}|,{\dot{\varphi }}_1,\gamma \}$ and $\underline{\dot{q}}$ can be found:

$$\underline{{\dot{q}}_P}=\left\{\begin{array}{c}{\dot{x}}_1\\ {\dot{y}}_1\\ {\dot{\varphi }}_1\\ \dot{\delta }\\ \dot{\gamma }\end{array}\right\}=\left[\begin{array}{ccc}cos\left({\varphi }_1\right)& 0& 0\\ sin\left({\varphi }_1\right)& 0& 0\\ 0& 1& 0\\ {\displaystyle -\frac{sin\left(\delta \right)}{b}}& 1& 0\\ 0& 0& 1\end{array}\right]\left\{\begin{array}{c}|\underline{v_1}|\\ {\dot{\varphi }}_1\\ \dot{\gamma }\end{array}\right\}$$

(8)

To obtain the linear mapping from $\underline{u}$ to $\underline{\dot{q}}$, the differential-drive model of the front module must be taken into account. Thus, the instantaneous center of rotation $ic{r}_F$ of the front module must lie on the ${\widehat{j}}_F$ axis, and the following relationship can be written:

$$|\underline{v_1}| = |\underline{v_{1,R}}| + \frac{|\underline{v_{1,L}}| - |\underline{v_{1,R}}|}{2}=\frac{1}{2}\left(\right|\underline{v_{1,L}}| + |\underline{v_{1,R}}\left|\right),$$

(9)

$${\dot{\varphi }}_1=\frac{|\underline{v_{1,R}}| + |\underline{v_{1,L}}|}{i}.$$

(10)

By substituting Equations (9) and (10) into (8), and taking into account the pure rolling hypothesis, the following final result is obtained:

$$\underline{{\dot{q}}_P}=\left[\begin{array}{ccc}{\displaystyle \frac{Rcos\left({\varphi }_1\right)}{2}}& {\displaystyle \frac{Rcos\left({\varphi }_1\right)}{2}}& {\displaystyle 0}\\ {\displaystyle \frac{Rsin\left({\varphi }_1\right)}{2}}& {\displaystyle \frac{Rsin\left({\varphi }_1\right)}{2}}& {\displaystyle 0}\\ {\displaystyle -\frac{R}{i}}& {\displaystyle \frac{R}{i}}& {\displaystyle 0}\\ {\displaystyle -\frac{R}{i}-\frac{Rsin\left(\delta \right)}{2b}}& {\displaystyle \frac{R}{i}-\frac{Rsin\left(\delta \right)}{2b}}& {\displaystyle 0}\\ {\displaystyle 0}& 0& 1\end{array}\right]\left\{\begin{array}{c}{\omega }_L\\ {\omega }_R\\ \dot{\gamma }\end{array}\right\}$$

(11)

2.3. Analytic Jacobian of the System

To combine the mobility of the mobile base and the manipulator and to plan the motion of the end effector of the mobile manipulator in ${\mathbb{R}}^6$ (position and orientation), the analytic Jacobian matrix of the system must be computed. The total set of generalized coordinates of the system $\underline{q}\in {\mathbb{R}}^{p+r}$ is composed of both the generalized coordinates of the platform $\underline{q_P}\in {\mathbb{R}}^p$ and the manipulator ones $\underline{q_R}\in {\mathbb{R}}^r$:

$$\underline{q}=\{\underline{q_P},\underline{q_R}\},$$

(12)

Although this general formulation can also be used with other mobile manipulator systems, it is worth recalling that for the Agri.Q prototype, $p=5$ and $r=7$.

The position and orientation of the end-effector (ee) of the mobile manipulator with respect to the fixed frame $\{O\}$ can be described by the 4 × 4 homogeneous transformation matrix $T_{ee}^0$ defined as follows:

$$T_{ee}^0\left(\underline{q}\right)=\left[\begin{array}{cc}{R}_{ee}^O\left(\underline{q}\right)& {\displaystyle \underline{p_{ee}^O}\left(\underline{q}\right)}\\ \underline{0}& 1\end{array}\right]$$

(13)

where $R_{ee}^O$ is the 3 × 3 rotation matrix describing the orientation of the end-effector wrt $\{O\}$, and $\underline{p_{ee}^O}$ is the associated 3 × 1 position vector. $T_{ee}^O$ can be computed as the product of three matrices, so that the mobility of the system is decomposed and studied individually:

$$T_{ee}^O\left(\underline{q}\right)=T_F^O{T}_R^F{T}_{ee}^R$$

(14)

where:

-

$T_F^O=T_F^O(x_1,y_1,{\varphi }_1)$ represents the transformation matrix from the fixed frame $\{O\}$ to $\{F\}$, fixed to the front module. It depends on the three degrees of freedom of the front module motion in the $\{{\widehat{i}}_O,{\widehat{j}}_O\}$ plane;

-

$T_R^F=T_R^F(\delta ,\gamma )$ represents the transformation from $\{F\}$ to $\{R\}$, which is the reference frame fixed to the manipulator base, and it depends on the relative yaw angle $\delta$ between the front and back modules and the pitch angle $\gamma$. It also contains information about the mounting parameters $x_0$, $y_0$, and $z_0$;

-

${\displaystyle T_{ee}^R=T_{ee}^R\left(\underline{q_R}\right)}$ describes the forward kinematics of the serial kinematic chain of the manipulator.

The analytic Jacobian of the system is formed by the $3\times (p+r)$ matrix $J_v$, which contains the linear mapping between the joint space velocity and the end effector linear twist space, and the $3\times (p+r)$ matrix $J_{\omega }$, which transforms $\underline{\dot{q}}$ into the end-effector angular twists:

$$\left\{\begin{array}{c}\underline{v_{ee}}\\ \underline{{\omega }_{ee}}\end{array}\right\}=J\left(\underline{q}\right)\underline{\dot{q}}=\left[\begin{array}{c}{J}_v\\ J_{\omega }\end{array}\right]\underline{\dot{q}}$$

(15)

The analytic expressions of $\underline{v_{ee}}$ and $\underline{{\omega }_{ee}}$ can be easily derived from $T_{ee}^O$:

$${\displaystyle \underline{v_{ee}}=\frac{\mathrm{d}\underline{p_{ee}^O}}{\mathrm{d}t},}$$

(16)

$$\underline{{\omega }_{ee}}=\{-S_{23},S_{13},-S_{12}\},$$

(17)

where S is the 3 × 3 skew-symmetric matrix defined as:

$${\displaystyle S={\dot{R}}_{ee}^O{R}_{ee}^{OT}}$$

(18)

As explained by De Luca et al. in [21], for motion planning purposes it could be useful to express the Equation (15) in terms of the command vector $\underline{u}=\{\underline{u_P},\underline{u_R}\}$, where $\underline{u_R}$, i.e., the command vector of the manipulator, coincides with the joint space velocity vector:

$${\displaystyle \underline{u_R}=\underline{{\dot{q}}_R}}$$

(19)

Thus, the $6\times (p+r)$ Jacobian matrix can be decomposed into the $6\times p$ matrix $J_P$, which describes the analytical Jacobian of the mobile platform and the 6 × r Jacobian $J_R$ of the manipulator. Thus, Equation (15) can be modified into:

$$\left\{\begin{array}{c}{\displaystyle \underline{v_{ee}}}\\ \underline{{\omega }_{ee}}\end{array}\right\}=J_P\underline{{\dot{q}}_P}+J_R\underline{{\dot{q}}_R}=J_{P}G\left(\underline{q_P}\right)\underline{u_P}+J_R\underline{{\dot{q}}_R},$$

(20)

where the matrix $G\left(\underline{q_P}\right)$ was defined in Equation (3) and calculated in (11).

3. Motion Planning Pipeline for Decoupled Motion

Even though the analytical Jacobian of the mobile manipulator can be used for motion planning using both the mobile base and manipulator mobility, exploiting the augmented capability to reach a target point, perform a task, or avoid obstacles, a simpler but still interesting way to move the system lies in the motion decoupling of the mobile base and the manipulator [22], motivated by the general higher position accuracy of the manipulator with respect to the base. Although such an approach does not allow the simultaneous motion of the base and the arm, it provides significant advantages in terms of motion planning, since it reduces the degrees of freedom that are involved during the two motions. A decoupled motion planning technique provides the great advantage of using the mobile base for a gross positioning of the manipulator, then exploiting the accuracy of the arm for grasping and manipulation.

Thus, a novel motion planning method that exploits the base and arm mobility of Agri.Q prototype for a pick and place task is presented and discussed.

In a generic scenario where Agri.Q moves inside the rows of vineyards for sampling crops or soil, the motion planning pipeline can be described as follows:

-

Start phase: a high-level command requires the execution of a given task, such as a pick-and-place execution, which corresponds to a crop sampling task;

-

First perception phase: according to the specifications indicated in the start phase, a perception system, e.g., a depth camera, recognizes the target and evaluates its position and orientation with respect to the manipulator base frame $\{R\}$;

-

Mobile base motion planning and execution: while the arm is still fixed, the mobile base is moved to enable the arm to reach the target and, moreover, to place the manipulator in a proper way;

-

Second perception phase: the perception system recomputes the new position and orientation of the target with respect to the arm base frame;

-

Arm motion planning and execution: while the base is now fixed, the arm performs the task taking care to not collide with the mobile base and the rest of the environment.

In the next subsections, the discussion is focused on the mobile base motion planning and execution phase, which contains the main contribution of the authors to the depicted pipeline. As mentioned before, the mobile base is moved in order to enable the arm to reach the target, so that it can also exploit its best manipulability area to perform the task. Then, the definition of the best manipulability area is presented, also with a closed form inverse kinematics technique that is able to evaluate the manipulability index of the arm. In the last subsection, we illustrate how the mobile base can properly position the arm.

3.1. Modified Manipulability Index of the Manipulator

A common approach to evaluate the dexterity of a robotic system is the use of the manipulability index defined as [23]:

$$c=\frac{1}{\kappa },$$

(21)

where $\kappa$ is the 2-norm condition number of Jacobian matrix $J_R$, calculated as the ratio between the highest and lowest eigenvalues of $J_R$, respectively, ${\lambda }_{max,min}$:

$$\kappa =\frac{{\lambda }_{max}}{{\lambda }_{min}}$$

(22)

In order to take care of the joint limits constraints, where the arm may have good dexterity even though it is close to the limit of its motion, Chan and Dubey proposed in [24] a gradient function that is used to penalize the Jacobian matrix $J_R$ in a given configuration of the arm, already implemented by Vahrenkamp et al. in [25] and by Chen et al. in [26]. The gradient function $\nabla h{\left({\theta }_j\right)}_j$ for the j-th joint is defined as:

$$\nabla {\left({\theta }_j\right)}_j=\frac{{({\theta }_{j,max}-{\theta }_{j,min})}^2{(2{\theta }_j-{\theta }_{j,max}-{\theta }_{j,min})}^2}{\alpha {({\theta }_{j,max}-{\theta }_j)}^2{({\theta }_j-{\theta }_{j,min})}^2},$$

(23)

where ${\theta }_{j,max}$, ${\theta }_{j,min}$ are, respectively, the upper and lower j-th joint limit, and $\alpha$ is a parameter that accentuates the closeness to the limit. The penalization factor for each column of $J_R$ is then defined as:

$$p_j=\frac{1}{\sqrt{1+|\nabla h{\left({\theta }_j\right)}_j|}}.$$

(24)

In Figure 5 the behavior of $p_j$ for different values of $\alpha$ is presented. As mentioned before, a larger value of $\alpha$ causes a relaxation of the joint limits closeness. For a small value of $\alpha$, $p_j=1$ in a small area near the value $(q_{j,max}-q_{j,min})/2$. According to [25], the value of the parameter $\alpha =4$ was selected.

Thus, the modified manipulability index $c_{mod}$ is evaluated as the 2-norm condition number of the modified Jacobian $J_{R,mod}$, where each entry $J_{i,j}$ of $J_R$ is modified according to the corresponding penalization factor:

$$J_{ijR,mod}=p_j{J}_{ij}i=1,\dots ,6j=1,\dots ,7$$

(25)

$$c_{mod}=\frac{1}{\kappa \left(J_{R,mod}\left(\underline{q_R}\right)\right)}$$

(26)

It is worth noting that $j=1,\dots ,7$, because $r=7$ for the Agri.Q prototype.

3.2. Manipulator Manipulability Mapping

In order to properly move the mobile base so that the arm can take advantage of its best manipulability properties, the value of $c_{mod}$ must be mapped inside the manipulator workspace. To this aim, the authors have implemented a closed form inverse kinematics algorithm that provides all the possible solutions as a function of a parameter ${\varphi }_R$ [27], which is the swivel or elbow angle of the manipulator [28,29]. The presented formulation treats the seven d.o.f. manipulator as a human-like arm, where, for a fixed shoulder and wrist position, the elbow can be rotated, while the orientation of the hand remains fixed. Thus, the kinematic structure of the manipulator can be modeled with a first spherical joint, centered in the shoulder, and a second similar joint, centered in the wrist, and the two points are connected by a revolute joint that represents the elbow mobility. For more details about the closed form inverse kinematics formulation, please refer to the related work presented by the author in [27].

An open-source copy of the inverse kinematics algorithm is available online (https://github.com/giocolucci/Jaco2SwivelIK; https://it.mathworks.com/matlabcentral/fileexchange/108419-jaco2swivelik, accessed in 21 March 2022). It was tested on a Dell XPS machine, with Ubuntu 20.04 LTS, and an Intel [email protected] GHz processor. The total execution time was approximately 1 ms when used with a single fixed elbow angle, and 100 ms when the solution with the highest manipulability index was extracted.

Thus, since the robotic arm has seven degrees of freedom, there are ${\infty }^1$ solutions for a given pose (position and orientation), and the algorithm enables evaluating the dexterity of the arm for every possible solution, eventually extracting the solution with the highest manipulability index.

An example of a manipulability map is presented in Figure 6, where the value of c and $c_{mod}$ was evaluated inside the yellow domain w for a fixed end-effector orientation, illustrated in the figure. Both the area where $c_{mod}>0.5$, in blue, and $c_{mod}>0.6$, in red, were significantly reduced due to the joint limit constraints, introduced by the penalization factor $p_j$ described in (24).

3.3. Mobile Base Motion Planning

Since it was designed for crop sampling applications inside a vineyard, the mobile base of Agri.Q prototype can be used to position the manipulator where it can actually reach the target and take advantage of its improved dexterity. To develop the proposed simplified motion planning pipeline, the assumption of no motion in ${\widehat{i}}_O$ direction was made, because it represents the perpendicular direction to the vineyard rows, as described in Figure 7. While navigating inside a vineyard, the distance between the manipulator and a target, e.g., a grape, is fixed and equal to the distance between the manipulator and the row. Even though the arm will take advantage of better dexterity if it moves closer to the target, this implies that also the mobile base will move closer to the rows, leading to potential collisions. Thus, the navigation of the mobile manipulator Agri.Q inside the vineyard rows can be simplified, and the base motion can be described by a reduced set of generalized coordinates:

$$\underline{q_P}=\{y_1,\gamma \}.$$

(27)

Thus, the reference frame fixed to the manipulator $\{R\}$ is moved by the value s defined as the difference between the final and initial Cartesian coordinates $y_1$:

$$s=y_{1,f}-y_{1,i},$$

(28)

then, the new r.f. $\{R^{\prime }\}$ is transformed into $\{R_f\}$ due to the pitch angle $\gamma$ (Figure 7).

Since the manipulability map is fixed to $\{R\}$, the motion planning pipeline described in the following is proposed by the authors:

-

Starting from a desired goal pose, described by the homogeneous transformation matrix $T_{goal}^O$ with respect to $\{O\}$, a slice of the entire workspace, parallel to the $\{{\widehat{j}}_O,{\widehat{k}}_O\}$ plane and passing through the P target point, is extracted, depicted in Figure 8a as the circle in black.

-

Inside the extracted domain, one can evaluate what portion of the workspace can be actually reached with the desired end-effector orientation. It is worth pointing out that, in general, the domain of the reachable points with a desired pose does not coincide with the manipulator workspace, which, instead, considers all the possible reachable points without an orientation specification. The boundaries of the modified workspace are described in Figure 8b with the red line.

-

One can evaluate whether the point P lies inside the modified workspace boundaries. If it does, the manipulator can actually reach the target, and no motion of the mobile base is requested. If it does not, the mobile base must move to reach the target point.

-

If a mobile base motion is requested, the reduced workspace domain is mapped in terms of the modified manipulability index $c_{mod}$, defined in Equation (26).

-

An optimal area where $c_{mod}>c_{mod,min}$, represented in the figure inside the gray line, is calculated as a portion of the modified workspace. Its manipulability barycenter coordinates C are calculated with respect to $\{O\}$:

$$\underline{r_C}=\frac{{\sum }_{i=1}^N{c}_{mod,i}\underline{r_i}}{{\sum }_{i=1}^N{c}_{mod,i}},$$

(29)

where $\underline{r_i}$$i=1,\dots ,N$ is the position vector of the i-th point inside the optimal area, and $c_{mod,i}$ is its modified manipulability index.

In the reported example, $c_{mod,min}=0.4$.

-

$\{y_1,\gamma \}$ are calculated in order to move C to the target point P. In general, the procedure implies the rotation of the optimal area due to $\gamma$.

Figure 8. (a) Manipulator workspace representation (red volume) and extraction of the plane of interest (blue). Since no motion is enabled along the $i_O$ axis, the x off-set of the plane with respect to $\{O\}$ is the x coordinates of the target point P. (b) Optimal manipulability area identification (inside the gray line). The total workspace is reduced due to the orientation specification, thus obtaining the area inside the red circle that, in general, does not coincide with the black circle in (a). The optimal area is then moved and rotated according to Figure 7.

Figure 8. (a) Manipulator workspace representation (red volume) and extraction of the plane of interest (blue). Since no motion is enabled along the $i_O$ axis, the x off-set of the plane with respect to $\{O\}$ is the x coordinates of the target point P. (b) Optimal manipulability area identification (inside the gray line). The total workspace is reduced due to the orientation specification, thus obtaining the area inside the red circle that, in general, does not coincide with the black circle in (a). The optimal area is then moved and rotated according to Figure 7.

4. Conclusions

The paper presented the kinematic modeling of the Agri.Q mobile manipulator prototype, designed for precision agriculture purposes. Starting from the subsystems’ mobility, the linear mapping from the input velocity commands to the linear and angular twists of the whole system was presented. The differential kinematic model, that is based on the no lateral wheel slipping assumption, is fundamental for singularity analysis and to implement traditional redundancy resolution methods, e.g., the Extended Jacobian or the Reduced Gradient. To perform useful pick-and-place tasks while navigating inside vine rows, a simplified motion planning algorithm, based on the motion decoupling was presented. Thus, the mobile base, less accurate than the manipulator, is used to position the arm, also taking advantage of its best manipulability area. The presented method lends itself to a simple and quick implementation on the real prototype, since the motion decoupling techniques significantly reduce the computational cost of the motion planning phase.