Position Selection and Collision-Free Path Planning for Fruit Picking Robots

Abstract

As the population continues to grow, the need for increased food production has become increasingly evident. However, the agricultural workforce is decreasing due to industrialization. To address this gap, numerous research efforts are underway to adopt robots for harvesting. While significant progress has been made, especially in fruit harvesting using robotic systems, there are still inefficiencies and areas where human intervention is necessary. This paper proposes an algorithm for selecting the position of a mobile manipulator for fruit harvesting using an inverse reachability map to achieve automation and enhance efficiency. Additionally, a method for collision-free path planning in joint space using the model predictive artificial potential field (MPAPF) algorithm is proposed. The effectiveness of the proposed position selection and path planning algorithms was validated through harvesting simulations and a collision-free path was validated in experiments.

1. Introduction

With the continuous increase in the global population, food production must also be increased. However, modern society is facing challenges in food production due to the decline in the agricultural workforce caused by industrialization. To address these issues, agricultural automation technology is advancing and innovations in the field of harvesting robots are expected to contribute to improving global food productivity [1,2,3].

Fruit harvesting with robots requires the precise identification of tree and target fruit locations. However, the irregular growth patterns of trees and fruits make them difficult to detect. Furthermore, it is crucial to avoid damaging branches or other harvestable items during the harvesting process. Fruit harvesting using robotic systems requires a computer vision system to detect target objects and the surrounding environment, along with a harvesting method for the manipulator that avoids causing damage to other parts of the tree [4]. To address these challenges, significant research has been conducted. Related works are roughly divided into two main areas: computer vision systems for detecting target fruits and obstacles, and collision-free path planning for the robotic manipulator to avoid damaging fruits and trees.

In robotic fruit harvesting, RGB-D cameras or Lidar sensors are mainly used. RGB-D cameras are commonly used for fruit recognition based on color information or for measuring the position of fruits [5,6,7,8,9]. Although RGB-D cameras are sensitive to surrounding lighting conditions, their advantages in cost and computational efficiency make them suitable for practical applications in agriculture [10]. For overall mapping of orchards or precise fruit shape measurements, Lidar is used [11,12,13,14,15,16,17]. High costs made it difficult to use in agriculture in the past, but advancements have made it more affordable and accessible. To overcome individual limitations and enhance overall accuracy, research that integrates multiple vision systems is underway.

Trees grow randomly in nature, and fruits typically cluster together. Because of these characteristics, a harvesting robot may collide with tree branches and nearby fruits. Therefore, it is crucial to consider the surrounding environment when generating robot paths to minimize damage to the tree, its branches, and nearby fruits during harvesting. Commonly used path planning algorithms for robot arm include the A*, rapidly exploring random tree (RRT) and artificial potential field (APF) algorithms. The A* algorithm has the advantage of reaching the destination while avoiding obstacles and finding the shortest path, but it has a drawback in that generating paths in three-dimensional space becomes computationally expensive due to an excessive number of grid points [18].

RRT generates paths by growing a tree through random sampling, leading to non-smooth paths and detours in three-dimensional spaces. The branches that are extended from detours can lead to inefficiency in path planning or computation time. The efficiency of path generation directly impacts the operational efficiency of harvesting robots. Thus, it requires modification to minimize computation time. An algorithm called AtBi-RRT was proposed in [19], based on the Bi-RRT algorithm [20], to accelerate path decision speed by simultaneously growing trees from the initial and the target positions, along with the application of a target gravity concept and an adaptive parameter adjustment method. The proposed algorithm was used in collision-free motion for a litchi-picking robot, with improved planning time. Another algorithm called tGSRT, incorporating the concept of target gravity for increased speed and a GA for path optimization, was proposed in [21] for litchi-picking robot path planning.

APF offers the advantages of simplicity and high speed computation, but it easily stuck in local minima and has oscillations in the generated paths. An algorithm called TO-RRT was proposed in [22], combining RRT and APF, utilizing a node-first search strategy to prevent the local minima problems caused by APF and employing the regression superposition algorithm to minimize computation time. The proposed algorithm was used for a citrus-picking robot. In [23], the IAPF was proposed, which is based on an artificial potential function incorporated with a sigmoid function element to avoid local minima.

Despite various developments in orchard mapping, fruit detection, path planning and automated systems over the past 30 years, currently developed harvesting robots are considered less practical than manual harvesting in most applications [24]. Significant research has been conducted on fruit harvesting robots; however, the question of where a harvesting robot should be positioned to efficiently harvest a fruit or a cluster of fruits has been overlooked. In [25], a fully integrated system was implemented for autonomous citrus harvesting, but positioning it for harvesting specific target fruits still relied on manual intervention. The positioning of harvesting robots has not been sufficiently studied, as it often relies on manual placement by humans [26], is constrained by pre-defined and highly structured environments, such as planar orchards [27], or excludes target fruits that are inaccessible due to fixed robot bases locations and limited approach angles [28]. Placing robots in appropriate positions not only reduces the risk of harvesting failure and collisions with obstacles but also shortens path generation time [29]. Furthermore, it is inefficient to move the entire robot each time harvesting takes place, in contrast to manipulating the harvest from a fixed position [30]. The efficiency of the harvesting process can be increased by maximizing the number of fruits harvested from a single location with arm motions alone.

This paper proposes a new approach to selecting an optimal position of a harvesting robot and the orientation of end-effector for harvesting multiple fruits based on an inverse reachability map. Pre-determining the position of the mobile manipulator and the orientation of the end-effector enables efficient harvesting of as many fruits as possible from a single position, without the need to move the entire system. With the proposed scheme, the success rate of harvesting can be increased, since it takes into account the manipulator workspace and obstacles in advance.

This paper also proposes an algorithm for collision-free path planning for robot systems called the model predictive artificial potential field (MPAPF). Typically, path planning for a manipulator using APF involves calculating the gradient of the potential function acting on the end-effector and computing the torques applied to each joint using the manipulator Jacobian matrix. Alternatively, a grid-based method is employed, moving the end-effector to the point with the lowest potential value in the vicinity. These methods have a limitation in that it is difficult to account for the manipulator workspace or joint limits because the path planning is performed in the end-effector space. In MPAPF, by planning the path in joint space, singularities can be avoided and joint limits can be easily considered. Furthermore, the potential field is formed for multiple points on the manipulator, so MPAPF can avoid collisions between the entire manipulator and obstacles. Additionally, the risk of falling into a local minimum is reduced due to the complexity of the potential field. The positioning and path planning assumes that the locations of the tree and fruit are known in advance. The entire harvesting process was implemented in a simulator with a model of an orchard, and collision-free paths in complex environments were verified in experiments. The main contributions of this paper as follows:

• A new method to select the position of the robot base and the orientation for approaching a target fruit of the end-effector is proposed for increased efficiency and reduced risk of failure in harvesting.
• A novel joint-space collision-free path planning method, the model predictive artificial potential field (MPAPF), is proposed to avoid singularity and to consider the joint limits and geometry of the manipulator. By optimizing potential values over multiple steps, it also reduces the risk of local minima.

The remainder of this paper is structured as follows: Section 2 covers the method of selecting a position using the inverse reachability map. Section 3 explains the collision-free path planning method using the MPAPF algorithm. Section 4 validates the algorithm with applications in harvesting simulations. Section 5 demonstrates experiments with collision-free paths and the conclusions are provided in Section 6.

2. Position and Orientation Selection

2.1. Reachability Map

The reachability map was generated by voxelizing the space and solving the inverse kinematics for all orientations where the end-effector can approach the respective voxel, adding up the reachability scores only when solutions within the joint limits exist. A pseudo-code for computing a reachability map is shown in Algorithm 1. The reachability map for a manipulator—Open-Manipulator Pro, manufactured by Robotis (shown in Figure 1)—is depicted in Figure 2, where red and blue respectively represent the maximum and the minimum values of the reachability score, with yellow in the middle. The space was voxelized using a grid size of 50 mm, covering 1.4(W) × 1.4(D) × 0.7(H) in meters, since the manipulator reach is 645 mm. The orientation for approaching the voxels was set to $\pi /10$ spacing for each axis within the range of $-\pi$ to $\pi$.

Algorithm 1 Reachability Map Construction

1: Set voxel size and voxelization   2: Initialize Reachability Map (RM)   3: for each voxel $V_i$ position ${\mathit{P}}_{e,i}(x_i,y_i,z_i)$ do   4:    Initialize reachability score: $RMs\leftarrow 0$   5:    for each orientation ${\mathit{R}}_{e,j}({\theta }_{xj},{\theta }_{yj},{\theta }_{zj})$ do   6:      Calculate inverse kinematics: $\mathit{\theta }\leftarrow \mathit{I}.\mathit{K}({\mathit{P}}_{e,i},{\mathit{R}}_{e,j})$   7:      if $\mathit{\theta }$ exists then   8:         if $\mathit{\theta }$ is within joint limits then   9:           Calculate manipulability index: $\mu$ 10:           Update score: $RMs\leftarrow RMs+\mu$ 11:         end if 12:      end if 13:    end for 14:    Save $\left(V_i,RMs\right)$ 15: end for 16: Normalize entire reachability scores

A reachability map is a collection of $\left({\mathit{P}}_{e,i}-{\mathit{P}}_b,{\mu }_i\right),{\mathit{P}}_{e,i}-{\mathit{P}}_b\in \mathit{S}$ where ${\mathit{P}}_{e,i}$ and ${\mathit{P}}_b$ denote the positions of the end-effector and the manipulator base, respectively. $\mathit{S}$ denotes the manipulator workspace and $\mu$ denotes the manipulability, an indicator on how easily the manipulator moves in all directions. In this work, the one proposed in [31] is used:

$${\mu }_i=\sqrt{{\displaystyle \frac{{\lambda }_{min}({\mathit{J}}_i{\mathit{J}}_i^{\mathrm{T}})}{{\lambda }_{max}({\mathit{J}}_i{\mathit{J}}_i^{\mathrm{T}})}}}$$

(1)

where ${\lambda }_{min}(·)$ and ${\lambda }_{max}(·)$ denote the minimum and the maximum eigenvalues and ${\mathit{J}}_i=\mathit{J}(\mathit{\theta }\left({\mathit{P}}_{e,i})\right)$ denotes the manipulator Jacobian at joint angle $\mathit{\theta }\left({\mathit{P}}_{\mathit{e},\mathit{i}}\right)$, with the inverse kinematics $\mathit{\theta }\left({\mathit{P}}_e\right)$.

2.2. Inverse Reachability Map

While the reachability map shows the reachable area of the end-effector with a fixed robot base, the inverse reachability map demonstrates where to position the robot base so that the end-effector can reach a specific target point. The target fruits focused in this paper are Kanpei, a citrus cultivar, which are cultivated by suspending them separately on support structures during growth to prevent them from falling due to their weight or branches breaking, as shown in Figure 3. Since their pedicels grow upward, the roll and pitch angles of the end-effector of the harvesting robot are assumed to be zero. In addition, because the robot base cannot be lifted up and down, it is assumed that its vertical coordinate is also zero.

Similar to generating the reachability map, the inverse reachability map is generated by voxelizing the space and varying the yaw angle of the end-effector position while solving the inverse kinematics. If the obtained joint angles satisfy the joint limits, the forward kinematics is solved using the obtained inverse kinematics solution to calculate the positions of the joints and check for collisions between the manipulator and obstacles. If there are no collisions, the manipulability score is calculated and stored. The same procedure is repeated for multiple targets, voxelizing the space around the centroid of the location of each target in a similar manner. Directly summing the individual maps could lead to issues where certain positions might have the highest score, despite there being no feasible solution for some fruit. To mitigate this, we assign negative values to positions where no solution exists and utilize the summed inverse reachability scores. If there is no common solution, different positions were selected for fruits. A pseudo-code for computing an inverse reachability map is shown in Algorithm 2. Once positions are determined, the end-effector orientation is selected with the highest single reachability score by varying the approach angle to the target location.

Algorithm 2 Inverse Reachability Map Construction

1: Set target $\mathit{T}(x_t,y_t,z_t)$, voxel size and voxelization   2: Initialize Inverse Reachability Map (IRM)   3: for each voxel $V_i$ position ${\mathit{P}}_{b,i}(x_i,y_i,0)$ do   4:    Initialize inverse reachability score: $IRMs\leftarrow 0$   5:    for each orientation ${\mathit{R}}_{e,j}(0,0,{\theta }_{zj})$ do   6:      Calculate inverse kinematics: $\mathit{\theta }\leftarrow \mathit{I}.\mathit{K}(\mathit{T}-{\mathit{P}}_{b,i},{\mathit{R}}_{e,j})$   7:      if $\mathit{\theta }$ exists then   8:         if $\mathit{\theta }$ is within joint limits then   9:           Calculate forward kinematics: $\mathit{F}.\mathit{K}\left(\theta \right)$ 10:           if Link points collide with obstacle then 11:              Calculate manipulability index: $\mu$ 12:              Update score: $IRMs\leftarrow IRMs+\mu$ 13:           end if 14:           $IRMs\leftarrow -1$ 15:         end if 16:         $IRMs\leftarrow -1$ 17:      end if 18:      $IRMs\leftarrow -1$ 19:    end for 20:    Save $\left(V_i,IRMs\right)$ 21: end for 22: Normalize entire inverse reachability scores

Figure 4 depicts an example of an inverse reachability map for a random target. When there are no obstacles for the same target fruit, the map appears symmetrically, as shown in Figure 4a. However, when obstacles exist, the IRM appears restricted, as shown in Figure 4b. The target fruit is depicted as a green circle, and the position with the highest score is marked with a black circle, where red and blue respectively represent the maximum and the minimum values of the inverse reachability score, with yellow in the middle. Examples of the map for 15 obstacles and five target fruits are presented in Figure 5. The space was voxelized using a grid size of 50 mm, with the centroid of the targets as its center. Voxelized space spans 1.4(W) × 1.4(D) in meters. The orientation for approaching a target fruit of the end-effector ranges from $-\pi$ to $\pi$, with a grid size of $\pi /8$ radians.

Figure 5a–e represent an inverse reachability map for each of the five target fruits, and Figure 5f shows the integrated inverse reachability map for all the five target fruits. The target fruits are depicted as green circles, and the positions with the highest score in individual maps are marked with black circles. In Figure 5f, the magenta circle denotes the position with the highest score. Figure 6 denotes the appearance of the manipulator reaching each target object at the selected position and the orientation of the end-effector, where the cyan circles denote obstacles. If the positions of obstacles in the orchard environment and the locations of the target fruits are known in advance, this algorithm can be applied to select the position of the mobile manipulator and fruit-harvesting orientation of the end-effector, enabling the harvesting of multiple fruits. Figure 7 shows the result of directly summing the individual maps. As in Figure 5, the highest score is indicated in magenta. As seen in Figure 7d, summing the individual maps without assigning negative values can result in the highest score appearing at locations that are not reachable for some targets.

3. Collision-Free Path Planning

3.1. Artificial Potential Field

Artificial potential field (APF) is a path planning method that uses a virtual potential field, inducing attraction towards the destination and repulsion near obstacles. Compared to other path planning methods, APF is widely used due to its simplicity and low computational requirements. The repulsive potential field for obstacle i, ${\mathrm{U}}_{rep,i}$, was constructed based on a quadratic form [32]:

$$\begin{array}{c}\hfill {\mathrm{U}}_{rep,i}\left(\mathit{P}\right)=\left\{\begin{array}{cc}{K}_r{\left({\displaystyle \frac{1}{D^{(}\mathit{P},{\mathit{P}}_{obs,i})}}-{\displaystyle \frac{1}{D_{min,i}}}\right)}^2& \mathrm{if}D(\mathit{P},{\mathit{P}}_{obs,i})\le D_{min,i}\\ 0& \mathrm{otherwise}.\end{array}\right.\end{array}$$

(2)

where $\mathit{P}$ represents the position of joint, ${\mathit{P}}_{obs,i}$ represents the position of obstacle i, $D(\mathit{P},{\mathit{P}}_{obs,i})$ is the euclidean distance between $\mathit{P}$ and ${\mathit{P}}_{obs,i}$, $D_{min,i}$ is the safety distance for obstacle i, and $K_r$ is the repulsive gain.

3.2. Model Predictive Artificial Potential Field

This paper introduces model predictive artificial potential field (MPAPF), a method that formulates a nonlinear optimization problem to plan paths in joint space using APF. This MPAPF path planning method has the advantage of avoiding the problem of generated paths exiting the manipulator workspace or not accurately considering joint limits. Additionally, collision checks are performed for joint positions as well, ensuring that all points of the manipulator do not collide with the obstacle. Using the APF values of states for multiple steps reduces the risk of local minima compared to APF methods. In the case of APF, the path may get stuck in a local minimum if there is no nearby position with a lower potential value than the current one. However, MPAPF finds a path by minimizing the cumulative potential value over multiple steps, allowing it to move to a higher-potential position if that helps it to escape from a local minimum.

The states at any discrete time instant within the prediction horizon n with sampling time $T_s$ and the angular velocity at time instant k, $u_k$, are expressed as follows:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathit{\theta }}_{k+1}& ={\mathit{\theta }}_k+T_s{\mathit{u}}_k\hfill \\ \hfill {\mathit{\theta }}_{k+2}& ={\mathit{\theta }}_{k+1}+T_s{\mathit{u}}_{k+1}={\mathit{\theta }}_k+T_s\left({\mathit{u}}_k+{\mathit{u}}_{k+1}\right)\hfill \\ \hfill & ⋮\hfill \\ \hfill {\mathit{\theta }}_{k+n}& ={\mathit{\theta }}_k+T_s\left({\mathit{u}}_k+{\mathit{u}}_{k+1}+\cdots +{\mathit{u}}_{k+n-1}\right)\hfill \end{array}\end{array}$$

(3)

Equation (3) can be simply expressed as follows:

$$\begin{array}{c}\hfill {\widehat{\mathit{\theta }}}_{k+1}=\mathit{A}{\mathit{\theta }}_k+\mathit{B}{\widehat{\mathit{u}}}_k\end{array}$$

(4)

where

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\widehat{\mathit{\theta }}}_{k+1}& ={\left[\begin{array}{cccc}{\mathit{\theta }}_{k+1}& {\mathit{\theta }}_{k+2}& \cdots & {\mathit{\theta }}_{k+n}\end{array}\right]}^{\mathrm{T}}\hfill \\ \hfill {\widehat{\mathit{u}}}_k& ={\left[\begin{array}{cccc}{\mathit{u}}_k& {\mathit{u}}_{k+1}& \cdots & {\mathit{u}}_{k+n-1}\end{array}\right]}^{\mathrm{T}}\hfill \\ \hfill \mathit{A}& =\left[\begin{array}{cccc}{\mathit{I}}_m& 0& \cdots & 0\\ 0& {\mathit{I}}_m& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮\\ 0& 0& \cdots & {\mathit{I}}_m\end{array}\right]\hfill \\ \hfill \mathit{B}& =T_s\left[\begin{array}{cccc}{\mathit{I}}_m& 0& \cdots & 0\\ {\mathit{I}}_m& {\mathit{I}}_m& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮\\ {\mathit{I}}_m& {\mathit{I}}_m& \cdots & {\mathit{I}}_m\end{array}\right]\hfill \end{array}\end{array}$$

(5)

where m is the number of joints, ${\mathit{\theta }}_k\in {\mathbb{R}}^m$ is the joint angle at time instant k and ${\mathit{I}}_m\in {\mathbb{R}}^{m\times m}$ is an identity matrix.

If ${\widehat{\mathit{\theta }}}_{k+1}$ are known, the positions of the joints, ${\mathit{P}}_j\left({\widehat{\mathit{\theta }}}_{k+1}\right)$, can be determined by the forward kinematics. A nonlinear minimization problem can be formulated as follows using the joint positions and angles, along with the positions of the obstacles:

$$\begin{array}{cc}\hfill \underset{{\widehat{\mathit{u}}}_k}{min}& \alpha f\left({\widehat{\mathit{\theta }}}_{k+1}\right)+\beta g\left({\widehat{\mathit{\theta }}}_{k+1}\right)\hfill \end{array}$$

(6)

where

$$\begin{array}{c}\begin{array}{c}\hfill f\left({\widehat{\mathit{\theta }}}_{k+1}\right)=\sum _{i=1}^o{\overline{\mathrm{U}}}_{rep,i}\left({\mathit{P}}_j\left({\widehat{\mathit{\theta }}}_{k+1}\right)\right)\\ \hfill {\overline{\mathrm{U}}}_{rep,i}\left({\mathit{P}}_j\left({\widehat{\mathit{\theta }}}_{k+1}\right)\right)=\sum _{j=1}^m{\mathrm{U}}_{rep,i}\left({\mathit{P}}_j\left({\widehat{\mathit{\theta }}}_{k+1}\right)\right)\\ \hfill g\left({\widehat{\mathit{\theta }}}_{k+1}\right)=|{\widehat{\mathit{\theta }}}_{k+1}-\mathit{C}·{\mathit{\theta }}_{target}|\\ \hfill \mathit{C}={\left[\begin{array}{cccc}{\mathit{I}}_m& {\mathit{I}}_m& \cdots & {\mathit{I}}_m\end{array}\right]}^{\mathrm{T}}\in {\mathbb{R}}^{m·n\times m}\end{array}\end{array}$$

where $\alpha$ and $\beta$ are constants representing the gains of each function, f is the obstacle avoidance function, g is the target attraction function, and o is the number of obstacles. ${\mathit{\theta }}_{target}\in {\mathbb{R}}^m$ denotes joint angles when reaching the target from the selected position. Function f uses a repulsive potential function, Equation (2), to avoid obstacles and g acts as an attractive potential to move all joints to the target angle.

In the manipulator used in this paper, each joint has different limits. To ensure that all the joints stay within lower and upper limits during the path planning process, ${\mathit{\theta }}_l,{\mathit{\theta }}_u\in {\mathbb{R}}^m$, the following constraints were added to the optimization problem:

$$\begin{array}{c}\hfill \mathit{C}·{\mathit{\theta }}_l\le {\widehat{\mathit{\theta }}}_{k+1}\le \mathit{C}·{\mathit{\theta }}_u\end{array}$$

(7)

Equation (7) can be expressed with the decision variable ${\widehat{\mathit{u}}}_k$, by using relation (3)–(5):

$$\begin{array}{c}{\mathit{\varphi }}_l\le {\widehat{\mathit{u}}}_k\le {\mathit{\varphi }}_u\\ {\mathit{\varphi }}_l=\mathit{C}·{\displaystyle \frac{1}{T_s}}\left({\mathit{\theta }}_l-{\mathit{\theta }}_k\right)\\ {\mathit{\varphi }}_u=\mathit{C}·{\displaystyle \frac{1}{T_s}}\left({\mathit{\theta }}_u-{\mathit{\theta }}_k\right)\end{array}$$

The nonlinear optimization problem formulated in Equation (6) was implemented in Python 3.7.6 using the SciPy 1.4.1 optimization library [33].

The computational time increases significantly as the number of obstacles increases due to collision checking for all joints and the nonlinearity of the objective function. To mitigate this, feasible groups of obstacles are represented as clusters. For example, in the case of a collision check problem with joints and obstacles, shown in Figure 8, collision checks are conducted between the joints of the manipulator, which are represented by red circles while the obstacles are represented by blue circles. In a problem with three joints and obstacles, Figure 8a requires nine collision checks between all the joints and obstacles. In contrast, it is sufficient to perform three collision checks only between the end-effector and the obstacles located within the clustered range, as shown in Figure 8b. The range of clusters is defined so that the repulsive potential is generated only when a joint is within this range, thereby reducing the amount of computations and thus minimizing the computational time. In this paper, the cluster range is defined by extending the minimum and maximum obstacle coordinates along each axis by the safety distance $D_{min}$ used in the repulsive potential. The path planning method using MPAPF was validated through simulation in Section 4 and experiments in Section 5.

3.3. Comparison to Other Algorithms

Common algorithms used in path planning for robotic manipulators include APF, RRT* and A*. The APF algorithm offers simplicity and fast computation for reaching a goal point but struggles with the inherent issue of local minima. The RRT* algorithm is flexible and adaptable to various environments by randomly expanding a tree, but it can produce inefficient paths and increase computation time due to detours. The A* algorithm guarantees an optimal path but has computational complexity, with excessive grids when dimensionality increases or grid size decreases.

Examples of path planning using the MPAF, APF, RRT* and A* algorithms are illustrated in Figure 9b, where a 2-link manipulator moves from the starting point, represented by a red triangle, to the goal point, represented by a green triangle, in an environment with obstacles represented by black circles, as shown in Figure 9a.

The movement trajectory of the manipulator for all the paths obtained in Figure 9b is shown in Figure 10. All algorithms except MPAPF either fail to find a solution due to exceeding the workspace or lead to collisions, as it is impossible to change from the initial elbow-up configuration to the goal elbow-down configuration. Collisions occur even when using the elbow-down configuration as the initial pose, as shown in Figure 11.

Table 1. Minimum distance and validity check for each algorithm.

Algorithm	Configuration	Link1 (m)	Link2 (m)	Collision	Workspace
MPAPF	EU/ED	0.1174	0.1169	Not occurred	Within
RRT*	EU	0.0687	0.0497	Occurred	Out
A*	EU	0.0354	0.0497	Occurred	Out
APF	EU	0.0537	0.0468	Occurred	Within
RRT*	ED	0.0430	0.0484	Occurred	Out
A*	ED	0.0354	0.0303	Occurred	Out
APF	ED	0.0537	0.0468	Occurred	Within

presents the results corresponding to Figure 10 and Figure 11. The safety distance was set to 0.05 m, and the table shows the minimum distances between each link center and the surrounding obstacles. Collisions occurred in all algorithms except MPAPF. In particular, when using RRT* and A*, the generated paths extend beyond the manipulator’s workspace, indicating that these methods are invalid for this task.

The generated paths must be converted to joint-space paths through post-processing. The velocity trajectory and the inverse Jacobian matrix are generally used to convert paths to joint space, since multiple solutions may exist when solving inverse kinematics directly for the generated path points. Even in this approach, the Jacobian matrix may not have an inverse depending on configuration of the manipulator leading to constraints in posture adjustment [34]. The Jacobian matrix may not be a square matrix when redundancies exist in the manipulator, making it difficult to achieve accurate conversion. Further, all of these algorithms, except for MPAPF, are point-based path planning methods and therefore cannot consider the geometry of the manipulator. Only the end-effector is considered fundamentally, leaving collisions at other points on the manipulator, joint limits, and the manipulator workspace entirely unaccounted for.

In contrast to point-based path planning algorithms, MPAPF allows the geometry of the manipulator to be considered through forward kinematics, enabling collision checking at all points on the manipulator and posture changes. A smooth path is generated without post-processing, as shown in Figure 10a, and since inverse kinematics is not used during path planning, the problem of multiple solutions does not need to be addressed.

The example potential maps using simple APF and MPAPF are shown in Figure 12. Red and blue respectively represent the maximum and minimum values of the potential field, with yellow in the middle. Virtual obstacles [35] or forces [36] to modify the potential map near local minima were proposed to address the local minimum problem. However, these methods can only help in local areas and a repetitive problem may occur where the object could return to the same spot. The potential map with MPAPF is formed in the joint space. The structure of the potential map with MPAPF is complicatedly transformed, as shown in Figure 12c, which helps reduce the probability of local minima. The paths are computed to minimize the sum of potential values over several discrete-time steps, allowing for movement toward areas with higher potential values if needed. This characteristic further reduces the risk of being trapped in local minima. Since the potential map expands depending on the number of joints, the complexity of the map increases as the number of joints increases. Figure 12b is for visualization purposes to illustrate the potential map in the $xy$-space workspace, so it may differ from the actual potential map.

4. Simulation

4.1. Simulation Setup

To validate the proposed methods, we constructed a simulator modeling a Kanpei orchard similar to real-world conditions. The simulator was built using GAZEBO and ROS kinetic on Ubuntu 16.04, running on a system with a 64-bit Intel(R) Core(TM) i7-7700K CPU running at 4.20 GHz. All The algorithms were implemented in Python.

The mobile manipulator consisted of Robotis’ RH-P12 gripper mounted on an Open-Manipulator pro, which was placed on Clearpath Robotics’ Ridgeback UGV equipped with Omni-directional wheels, as shown in Figure 13. The Kanpei trees were strategically placed inside obstacles formed by other trees to complicate the approach for the mobile manipulator. The fruits were modeled to resemble their real counterparts, with a diameter of approximately 10 cm. The modeling of Kanpei trees and the orchard is illustrated in Figure 14.

It was assumed that the positions of all the trees and fruits were known through orchard mapping via a vision system. The harvesting scenario has two parts: First, the closest three fruits to the initial position of the robot were selected for harvesting. Fixed positions of UGV and the orientation for approaching a target fruit of the end-effector for the target fruits were selected using IRM. Then, the UGV path to the selected position and harvesting paths of the manipulator for each fruit from that position were generated using MPAPF and executed. Second, the same procedure was repeated for harvesting three fruits placed behind trees. Harvesting paths of the manipulator were generated only from initial basket placement to fruits and repeated to return to the initial in reverse.

4.2. Simulation Result

The simulation results using the implemented simulator and algorithms are illustrated in Figure 15. The first part is shown in Figure 15a–d, and the second part in Figure 15e–h. The mobile base moves to the selected position, as shown in Figure 15a, and then the manipulator harvests the target fruits, as shown in Figure 15b–d. The same procedure was repeated for other targets, as shown in Figure 15e–h. The generated paths for the UGV are shown in Figure 16. In each simulation part, the initial positions are represented in red, the selected final positions from IRM in green, and the mapped obstacle trees in brown circles. The intermediate paths from the initial position to the selected final position were generated by MPAPF and are represented in blue. The computation times for position and orientation selections were 2.064 and 1.831 s for the first and second parts, respectively. The path planning computation times were 0.114 and 0.04 s for the first and second parts, respectively. Ten and six path points were generated, and the paths were set up for the mobile base to pass through the points sequentially without generating trajectories.

The harvesting paths of the manipulator for each part of harvesting are shown in Figure 17. The paths for harvesting three fruits in each part are shown in red, green, and blue, respectively. The target fruits are represented by the orange circles, while other obstacle fruits are represented by cyan circles. Each generated path ensures collision-free reaching of the target fruits without movement of the base. The computation times for the paths were 2.985, 2.623, 3.923, 2.815, 2.847 and 4.054 s, and the number of path points generated were 84, 74, 110, 84, 82 and 112 for the path planning of each of the six target fruits. The joint angles of the manipulator are shown in Figure 18, where the black and blue solid lines represent the joint angles for the first and second part, respectively, and the red dashed line indicates the joint limits. All the paths were generated to ensure that the joint angles did not exceed their limits.

5. Experiment

5.1. Experiment Setup

To validate the collision-free path planning using the proposed MPAPF, experiments were conducted in a complex situation only for collision-free path planning. The manipulator was fixed at an arbitrary position without a mobile base due to the limitations of the experimental environment. As shown in Figure 19, the experiment environment consisted of a shelf with four targets, one of which was placed behind a partition to make it difficult to approach.

The experiments were conducted under the assumption that the positions of obstacles and targets were known without sensors for detecting the environment. Both the shelf and the partition were represented in the form of point clouds. As mentioned in Section 3, in order to reduce computation time, all shelf parts and partition were represented as clusters. The distance threshold between obstacle clusters and joints was set to 100 mm and the safety distance for the repulsive potential was set to 50 mm. All point clouds were structured at intervals of 50 mm and 110 points for the shelf and 44 points for the partitions, respectively. The position selection algorithm was not applied and only the experiment verification of collision-free path planning was conducted. The target object arrival orientations were arbitrarily set. The paths from the initial posture to each target were generated using MPAPF and were organized to return to the initial position in reverse. The same manipulator and system were used in the experiment as in the simulator. The experiment was conducted by directly inputting the joint trajectory calculated on the PC into the manipulator through U2D2 communication converter by Robotis. The manipulator was equipped with an integrated device—DYNAMIXEL by Robotis—which allows the user to input an angle value, enabling the robot to follow it without requiring a controller or driver. The overall environment system diagram is shown in Figure 20.

5.2. Experiment Result

The experiment results of applying collision-free path planning using MPAPF to the manipulator are shown in Figure 21. In Figure 21a, the initial posture is depicted, and in b–d, the manipulator reaches the relatively easily accessible target objects without colliding with the shelf. In e–h, the manipulator approaches the target located behind the partition without colliding with the shelf or the partition. The path computation times were 4.9, 3.8, 4.8 and 45.2 s, respectively. Since the final target is located within the shelf and partition, the manipulator frequently remained closer than the threshold. The computation time was prolonged when approaching the final target due to the dense configuration of the point cloud. Each path consists of 42, 32, 42 and 78 path points. In experiment, the manipulator was operated with a 0.25 s interval between generated path points. The generated paths are shown in Figure 22. The paths to each target object are indicated in red, green, blue and cyan, respectively. As shown in Figure 22, none of the paths overlap with the shelf or partition. Joint angles are shown in Figure 23, where all the generated joint angles are within the joint limit range. The clearances in Figure 24 represent the distances between the positions of all the joints and the obstacles during manipulation, confirming that all parts of the manipulator did not approach closer than the set safety distance.

6. Conclusions

In this paper, research on the automation of Kanpei harvesting robots has been conducted. Using the proposed inverse reachability map based algorithm for selecting fruit harvesting positions, it is possible to pre-select advantageous robot system positions for harvesting. This allows for multiple target fruits to be harvested from one fixed location without the overall movement of the robot system, resulting in time and energy savings. Although the fruits to be harvested still need to be determined by humans, it is advantageous to know in advance whether it is possible to harvest using only the manipulator without moving the base. And MPAPF was proposed for collision-free path planning that not only considers the end-effector but also the manipulator as a whole. Since the path is planned in joint space, the end-effector does not exceed the workspace, and the joint limits are also considered. By adding the target angle to the objective function and calculating the potential for multiple points, the problem of falling into local minima, which is a persistent issue encountered with APF, can be mitigated to some extent. Through simulations, successful harvesting of multiple fruits is possible using the position selection and path planning algorithms. Furthermore, experiments showed that collision-free paths are well-planned even in complex environments. However, the computation time increases significantly as the number of obstacles increases; therefore, improvements in the algorithm will be further pursued. Moreover, the entire system, including the mobile base, a vision system, and new types of end-effector, will be developed for experimental validation in a real Kanpei harvesting environment.