Retrieval Augment: Robust Path Planning for Fruit-Picking Robot Based on Real-Time Policy Reconstruction

Abstract

The working environment of fruit-picking robots is highly complex, involving numerous obstacles such as branches. Sampling-based algorithms like Rapidly Exploring Random Trees (RRTs) are faster but suffer from low success rates and poor path quality. Deep reinforcement learning (DRL) has excelled in high-degree-of-freedom (DOF) robot path planning, but typically requires substantial computational resources and long training cycles, which limits its applicability in resource-constrained and large-scale agricultural deployments. However, picking robot agents trained by DRL underperform because of the complexity and dynamics of the picking scenes. We propose a real-time policy reconstruction method based on experience retrieval to augment an agent trained by DRL. The key idea is to optimize the agent’s policy during inference rather than retraining, thereby reducing training cost, energy consumption, and data requirements, which are critical factors for sustainable agricultural robotics. We first use Soft Actor–Critic (SAC) to train the agent with simple picking tasks and less episodes. When faced with complex picking tasks, instead of retraining the agent, we reconstruct its policy by retrieving experience from similar tasks and revising action in real time, which is implemented specifically by real-time action evaluation and rejection sampling. Overall, the agent evolves into an augment agent through policy reconstruction, enabling it to perform much better in complex tasks with narrow passages and dense obstacles than the original agent. We test our method both in simulation and in the real world. Results show that the augment agent outperforms the original agent and sampling-based algorithms such as BIT* and AIT* in terms of success rate (+133.3%) and path quality (+60.4%), demonstrating its potential to support reliable, scalable, and sustainable fruit-picking automation.

1. Introduction

In agriculture, picking tasks require robots to recognize target fruit and obstacles, and plan a collision-free path connecting the start posture and target posture. A robust picking strategy must address both grasping posture decision and path planning, presenting challenges to algorithmic efficiency and accuracy due to the different fruits’ varying spatial postures and shapes. With increasing labor shortages, rising production costs, and the demand for reduced environmental impact, efficient and intelligent harvesting robots have become a key enabler for sustainable agricultural production. Researchers have proposed solutions such as RRT, A*, and APF [1,2,3]. Sampling-based algorithms, including RRT and its improved versions like RRT_connect [4], RRT* [5], are widely used for their computational efficiency. Hemming et al. [6] applied RRT for autonomous pepper picking, but their approach was limited to static environments with minimal interactions. Wang et al. [7] utilized RRT with polynomial trajectory optimization for tomato picking, achieving an 88% success rate, albeit with a high time cost (20 s per task). However, these methods are challenging to apply to high-DOF robots, as they require precise environment descriptions, which lead to exponential increases in computational complexity as the robot’s DOF increases [8].

Recent advancements in DRL have shown considerable potential for path planning in high-DOF robots [9,10,11]. DRL algorithms such as SAC [12], which update policy through experience replay and reward mechanisms, train the action policy network to solve the Markov Decision Process (MDP) for picking tasks. Chiang et al. [13] integrated DRL with classical RRT to improve path planning efficiency. Wang et al. [14] formulated agricultural picking sequence planning as a three-dimensional TSP and solved it using a pointer network-based actor–critic DRL framework. However, because the robot does not fully learn the real-world environment, it often encounters unforeseen obstacle zones, resulting in movement failures. Li et al. [15] developed a DRL-based harvesting strategy for clustered kiwifruits, in which fruit recognition and localization as well as picking order planning are first performed, followed by DRL-based optimization to generate an efficient harvesting strategy. Yi et al. [16] proposed a self-supervised DRL-based view planning method that employs a Self-Supervised Convolutional Network to evaluate the effectiveness of actions during training and dynamically adjust rewards to guide policy learning. Liu et al. [17] proposed an expert experience-based guided reinforcement learning strategy for high-DOF robotic arms in apple picking, outperforming RRT in planning time and path quality. Feng et al. [18] introduced an enhanced HER-SAC algorithm that incorporates a heuristic action fusion strategy during training to optimize cherry tomato picking. Their work addresses grasping posture decisions more thoroughly, and is referenced in the training of the original agent (Section 3.2) in this paper. Despite the promising potential of DRL, its application in agricultural picking tasks remains constrained by the substantial computational cost required for training and the limited adaptability of offline policies to dynamically changing environments. Existing optimization approaches, such as retrieval-based reinforcement learning methods, typically incorporate human experience directly into the training loop, which further increases training complexity and computational burden. Moreover, updating the experience repository generally necessitates retraining the policy network, resulting in additional computational overhead and high overall training costs. Such resource-intensive learning paradigms are difficult to sustain in long-term, large-scale agricultural systems, where computational efficiency, energy consumption, and deployment cost are critical considerations.

In this paper, we present a decision framework based on real-time policy reconstruction to augment DRL-trained agent, enabling robust picking path planning in complex picking scenes (Figure 1). By shifting policy optimization from the training phase to real-time inference, the proposed framework reduces the need for repeated retraining and supports more energy- and data-efficient robot operation, aligning well with the requirements of sustainable agricultural automation. Retrieval augment, originating from the use of external memory in natural language processing (NLP) to improve large language models [19], has also been employed by Peter C. et al. [20] in large-scale verification to enhance DRL agent performance in Go scenarios. In contrast to policy correction or residual reinforcement learning methods, which typically require learning an additional corrective policy or residual model, our approach performs explicit policy reconstruction at the action level. Specifically, the final action decision is generated through real-time evaluation, weighted fusion, and rejection sampling between the original policy actions and experience-generated candidate actions. This process does not rely on any learnable correction model and does not introduce new optimization objectives during deployment. Furthermore, compared with planning-guided reinforcement learning methods, the proposed framework neither requires an explicit system dynamics model nor performs online trajectory optimization. Instead, it leverages an offline-constructed experience base and its probabilistic modeling results to provide high-quality candidate actions for the current state under strict real-time constraints. Our framework first regresses pre-collected, high-quality empirical data to summarize the robot’s performance in typical complex scenes, thereby constructing an experience base. When faced with difficult picking tasks, the original agent can reconstruct their action policy by retrieving the experience base, evolving into an augment agent capable of adapting to dynamic and complex environments. The main contributions of this paper are as follows:

1.

Propose a decision framework for real-time policy reconstruction based on experience retrieval.

2.

Propose a new method for collecting and characterizing high-quality robot empirical data.

3.

Instantiate a specific policy reconstruction scheme: an action fusion method based on real-time evaluation and rejection sampling is proposed for policy reconstruction.

2. Problem Formulation

We consider path planning for a picking task, which requires planning a collision-free path from the starting position to the optimal cutting position in the workspace and maximizing the path quality (path length and smoothness). Specifically, we define this problem as follows.

As shown in Figure 2, we consider the robot R and the goal $G=\{f,k,b_1,b_2\}$ within the same bounded workspace $\chi \in R^3$. The goal G consists of the fruit f ($kg$ and cube F), the operation point k, the target branch $b_1\left(k{k}^{\prime }\right)$, and the main branch $b_2\left(mn\right)$. The workspace is divided into free space ${\chi }_{free}$ and obstacle space ${\chi }_{obstacle}$, where obstacles are generalized as 3D entities in $\chi$, excluding the target branch. We define the robot’s state $s\left(t\right)$ at time t as its joint angles $\mathit{\theta }=({\theta }_1,\cdots ,{\theta }_n)$ in configuration space $Q\in R^n$, and the end-effector pose $T=(p,q)$ in Cartesian space, with $p=(x,y,z)$ as the 3D position and $q=(q_w,q_x,q_y,q_z)$ as the quaternion representing the rotation. The set of valid states of the robot is defined as $\mathcal{S}=\left\{s(\mathit{\theta },T)\right|collision\left(s\right)=0,\forall {\theta }_{i}in\mathit{\theta }:{\theta }_i^{min}<{\theta }_i<{\theta }_i^{max}\}$. The starting pose of the robot is $s_0=({\mathit{\theta }}_0,T_0)$, and the target pose is the optimal cutting pose $s^{*}=({\mathit{\theta }}^{*},T^{*})$. The optimal cutting pose satisfies the conditions $s^{*}\in \mathcal{S},d(p^{*},k)\le \delta$, and ${\mathbf{v}}_H^Z·{\mathbf{v}}_{b_1}\le ϵ$, where $\mathbf{k}$ is the unit vector pointing from the world coordinate origin to the operation point k. ${\mathbf{v}}_H^Z$ is the unit vector in the direction of the Z-axis of the end-effector’s reference frame in the world coordinate system, and ${\mathbf{v}}_{b_1}$ is the unit vector in the direction of branch $b_1$. Here, $\delta$ and $ϵ$ are small constants to ensure the end effector is sufficiently close to k and the orthogonal cutting posture. According to the definition, the optimal cutting pose is not unique; i.e., there exists a set $S^{*}=\left\{s^{*}|s^{*}\in \mathcal{S},d(p^{*},k)\le \delta ,{\mathbf{v}}_H^Z·{\mathbf{v}}_{b_1}\le ϵ\right\}$. We define the feasible path $c_i(s_{\mathrm{start}},s_{\mathrm{goal}})$, and connecting states $s_{\mathrm{start}}$ and $s_{\mathrm{goal}}$ are defined as a sequence of states, i.e., $c_i(s_{\mathrm{start}},s_{\mathrm{goal}})=(s_1^i,s_2^i,s_3^i,\dots ,s_m^i)$, where $s_1^i=s_{\mathrm{start}}$, $s_m=s_{\mathrm{goal}}$, and $s_j^i\in \mathcal{S}$.

We observe that there exists a set of paths that connect the starting position and the optimal cutting position, i.e., $\mathcal{C}=\left\{c(s_0,s^{*})|s^{*}\in S^{*}\right\}$. It is important to note that we do not intend to traverse the set $\mathcal{C}$ to find the optimal path, which would be time- and computationally expensive. Instead, our goal is for the robot to autonomously plan a path $c_i(s_0,s^{*})$ on the first attempt, satisfying $c_i\in \mathcal{C}$, while minimizing the path cost function $J\left(c\right)$. In this paper, we define the cost function as $J\left(c\right)={\sum }_{0<i<i+1<m}f(i,i+1)$, where m is the length of the path sequence, and $f(i,i+1)=w_1·d(p_i,p_{i+1})+w_2·\sqrt{{\sum }_k^{nJoint}{\left({\theta }_i^k-{\theta }_{i+1}^k\right)}^2}$ represents the evaluation of the cost for the transition from $s_i$ to $s_{i+1}$.

We classify picking tasks into simple ${\mathcal{G}}_1$ and complex ${\mathcal{G}}_2$ categories (as shown in Figure 2). Let the operation point k serve as the center of a square region Z with side length r, which defines the area of interest. The obstacle percentage within this region is calculated as $p_{obs}={\displaystyle \frac{V_{obs}}{V_{sphere}}}$. We designate the half-square of Z oriented toward the robot as the operational space $Z_0$. From the outer boundary of $Z_0$, m parallel rays ${\left[r_i\right]}_{i=1}^m$ are emitted uniformly in the vertical direction. Rays that encounter obstacles within a threshold distance $r_0$ are labeled $r_{obs}$. These rays are then projected onto a grid roadmap $M_{grid}$, where grids corresponding to $r_{obs}$ are marked as obstacle grids. The Manhattan distances ${\left\{d_i\right\}}_{i=1}^n$ from k to each obstacle grid are recorded, and their average is computed as $d_{ave}={\displaystyle \frac{{\sum }_{i=1}^n{d}_i}{n}}$, with n being the number of obstacle grids. A scene is defined as complex if $p_{obs}>p$ and $d_{ave}<\gamma$, where p and $\gamma$ are thresholds that ensure complex scenes are characterized by dense obstacles and narrow passages. Consequently, the set of goals $\mathcal{G}$ is partitioned into simple scenes ${\mathcal{G}}_1$ and complex scenes ${\mathcal{G}}_2$, i.e., $\mathcal{G}={\mathcal{G}}_1\cup {\mathcal{G}}_2$.

Given the extensive use of mathematical notation in this paper, all symbols appearing in this section and subsequent sections are summarized and explained in Appendix A.

3. Retrieval Augment: Real-Time Policy Reconstruction

This section presents our method, starting with an overview in Section 3.1, followed by a detailed description of each component.

3.1. Overview

The entire decision framework is realized as shown in Figure 3.

3.1.1. Original Agent Training

The policy reconstruction proposed in this paper begins with the pre-training of a picking robot agent. At this stage, the focus is not on achieving optimal performance across all picking tasks, as a strong agent presents significant challenges in terms of computational resource and DRL algorithms. Instead, the approach emphasizes enhancing the agent’s performance through retrieval augment, without further training. To achieve this, we extract simple scenes from ${\mathcal{G}}_1$ as training tasks and train the agent with fewer episodes (see Section 3.2 and Experiment for details).

3.1.2. Experience Base

Constructing an experience base is significant for effective policy reconstruction. In this paper, we adopt a scene-experience bundle model, where each experience corresponds to a typical challenging scene. For a typical scene $G\in {\mathcal{G}}_2$, we propose a raw data collection method based on hierarchical collaborative path exploration. First, the workspace $\chi$ is decomposed and initially explored to identify risk regions R via a scoring mechanism. Subsequently, a deeper exploration is conducted in the Cartesian space within R, where a reward function weights the data to yield the raw dataset $\mathcal{D}=\left((T_1,w_1),(T_2,w_2),\dots ,(T_n,w_n)\right)$, with each $(T_i,w_i)$ representing the end-effector pose (position ${\mathit{p}}_i$ and orientation ${\mathit{q}}_i$) and $w_i$ denoting its weight. $\mathcal{D}$ is divided into ${\mathcal{D}}_p=\left((p_1,w_1),(p_2,w_2),\dots ,(p_n,w_n)\right)$ and ${\mathcal{D}}_q=\left((q_1,w_1),(q_2,w_2),\dots ,(q_n,w_n)\right)$. A weighted Expectation Maximization Algorithm (EM) is then applied to ${\mathcal{D}}_p$ and ${\mathcal{D}}_q$ to derive Gaussian Mixture Model (GMM) $G_p$ and $G_q$. Finally, the scene, risk region, and sampling models are integrated into an experience: $E_i=(G_i,R_i,G_p^i,G_q^i)$. Note that q in ${\mathcal{D}}_q$ represents a quaternion. Directly applying a standard GMM in Euclidean space would ignore the intrinsic manifold structure of unit quaternions on $S^3$, thereby destroying their inherent geometric constraints and leading to invalid samples. To avoid data distortion and improve sampling efficiency, we first map q to its tangent space to preserve its geometric constraints. We then perform EM algorithm iterations in this tangent space. Finally, the standard quaternion is recovered by applying the inverse mapping to the sampling result (see Section 4.3 for details).

3.1.3. Experience Retrieval

When the original agent encounters a task scene $G\in {\mathcal{G}}_2$, it will extract the scene features and retrieve the most similar scene from the experience base. Following the method described in Section 3, we compute $p_{obs}$, unit vectors $v_{b1}$ and $v_{b2}$, and the grid roadmap $M_{grid}$, which is then mapped to a matrix M with elements 0 or 1. The similarity between $G_1$ and $G_k$ is computed using the following Equation:

$$\mathrm{SIM}(G_i,G_k)=w_{H_u}{\displaystyle \frac{H_i·H_k}{\parallel H_i\parallel ·\parallel H_k\parallel }}+w_{b_1}{v}_{b_1}^i·v_{b_1}^k+w_{b_2}{v}_{b_2}^i·v_{b_2}^k+w_p\left(1-\left|p_{\mathrm{obs}}^i-p_{\mathrm{obs}}^k\right|\right)$$

(1)

where $H_1,H_k$ are the Hu moments of the matrices $M_1,M_k$, and the weights satisfy $w_{H_u}+w_{b1}+w_{b2}+w_p=1$. For a task scene G, we obtain that $G^{*}$ satisfies $G^{*}={argmax}_{G^{*}\in {\mathcal{G}}_2}SIM(G,G^{*})$. Extract the experience $E^{*}$ for policy reconstruction.

Notably, the design objective of the similarity metric is to capture task-level geometric relevance rather than to serve as a learnable module. Specifically, Hu moments are employed to characterize the global distribution of obstacles, the branch orientation term encodes constraints directly related to the cutting posture in picking tasks, and the obstacle distance reflects the narrowness of local passageways. The weighting scheme is intended to balance these complementary geometric features, while normalization ensures their comparability in the similarity computation. In our experiments, the weights are set to $w_{H_u}=0.3,w_{b1}=0.2,w_{b2}=0.2$, and $w_p=0.3$. The experimental results in Section 4.2 indirectly validated this design. It shows that the similarity of retrieved experiences tends to stabilize as the size of the experience base increases. Moreover, subsequent experimental results indicates that the performance of policy reconstruction is more sensitive to the quality of experience similarity itself than to the specific numerical values of the weights.

3.1.4. Real-Time Policy Reconstruction

The two key elements of policy reconstruction are the action policy ${\pi }_{\theta }(a_t\mid s_t)=\mathcal{N}\left(a_t\mid {\mu }_{\theta }\left(s_t\right),{\sigma }_{\theta }\left(s_t\right)\right)$ of the original agent, and experience $E^{*}=(G^{*},R^{*},G_p^{*},G_q^{*})$. We define policy reconstruction process as $\mathcal{RC}\left({\pi }_{\theta },E^{*},t\right)={\pi }^{*}(a_t\mid s_t)$, which evolves the original agent into an augment agent. Notably, $\mathcal{RC}$ exhibits varying performance at different time step t. The simplest case occurs when reconstruction is not required in the current state, causing ${\pi }^{*}$ to revert to the original policy ${\pi }_{\theta }$ (see Section 4.3 for details).

3.2. Original Agent Training

3.2.1. Observation Space

The observation is described by the following: (a) The joint angles $\mathit{\theta }=[{\mathit{\theta }}_1,{\mathit{\theta }}_2,\cdots ,{\mathit{\theta }}_n]$. The robot in this paper is a 7-DOF robotic arm, so $\mathit{\theta }\in R^7$; (b) posture of end-effector $\mathit{T}=[\mathit{p},\mathit{q}]\in R^7$; (c) Euclidean distance $\mathit{d}$ of the end-effector reference system origin from the target operating point; (d) the angle $\alpha ={\displaystyle \frac{{\mathbf{v}}_H^z·{\mathbf{v}}_{b_1}}{\parallel {\mathbf{v}}_H^z\parallel \parallel {\mathbf{v}}_{b_1}\parallel }}$ between the unit vector in the direction of z-axis of the end-effector reference system and that in the direction of the target branch $b_1$; (e) unit vector $v_{b2}$ in the direction of the main branch $b_2$; (f) binary number representation of the collision detection result. As described above, the observed at the time step t is represented as a 20-dimensional vector:

$$s\left(t\right)=\left[\mathit{\theta },x,d,\alpha ,{\mathbf{v}}_{b_2},\mathrm{collision}\right]\in {\mathbb{R}}^{20}$$

(2)

This 20-dimensional observation space vector needs to be normalized.

3.2.2. Action Space

Action is described by the desired difference in position and orientation represented by $\dot{p},\dot{q}$, respectively:

$$a\left(t\right)=[\dot{p},\dot{q}]\in {\mathbb{R}}^7$$

(3)

3.2.3. Reward Function

The goal is to guide the robot to autonomously plan a path from the starting pose to the optimal cutting pose and to maximize the quality of the path. Based on this goal, we design five reward functions to reward or penalize the action at each step. $r_{distance}$ guides the robot to approach the operation point. $r_{pos}$ guides the robot to adjust its posture to the optimal grasping posture. $r_{traj}$ guides the robot to plan a smooth path as much as possible. $r_{collision}$ monitors whether the path planned by the robot is collision-free. Considering the design of action space, the next state after performing the action may be the singularity of the robot. $r_{valid}$ is designed to guide the policy network to output actions for which the inverse solution exists.

$$r_{distance}=-{\displaystyle \frac{1}{2}}{d}^2$$

(4)

$$r_{\mathrm{pos}}=-{\left({cos}^{-1}\left({\displaystyle \frac{{\mathbf{v}}_H^z·{\mathbf{v}}_{b_1}}{\parallel {\mathbf{v}}_H^z\parallel \parallel {\mathbf{v}}_{b_1}\parallel }}\right)\right)}^2=-{\left(\alpha -{\displaystyle \frac{\pi }{2}}\right)}^2$$

(5)

$$r_{\mathrm{traj}}=-\sqrt{\sum _{i=1}^{n_{\mathrm{joints}}}{\left({\theta }_t^i-{\theta }_{t-1}^i\right)}^2}$$

(6)

$$r_{\mathrm{collision}}=\left\{\begin{array}{cc}-100,\hfill & \mathrm{if}\mathrm{collision},\hfill \\ 0,\hfill & \mathrm{otherwise},\hfill \end{array}\right.$$

(7)

$$r_{\mathrm{valid}}=\left\{\begin{array}{cc}-100,\hfill & \mathrm{if}\mathrm{invalid},\hfill \\ 0,\hfill & \mathrm{otherwise}.\hfill \end{array}\right.$$

(8)

Finally, the reward at each step is computed by

$$r_t(s_t,a_t)={\lambda }_1{r}_{\mathrm{distance}}+{\lambda }_2{r}_{\mathrm{pos}}+{\lambda }_3{r}_{\mathrm{traj}}+r_{\mathrm{collision}}+r_{\mathrm{valid}}.$$

(9)

In this paper, the values of ${\lambda }_1,{\lambda }_2$ and ${\lambda }_3$ are set to ${\displaystyle \frac{1}{3}}$.

3.3. Experience Base

We propose a data collection method based on hierarchical collaborative exploration. To ensure the integrity of quaternion data representing the state of end effector, we employ a tangent space mapping during regression and an inverse mapping during sampling. This approach prevents distortions that arise from directly applying quaternion manifolds to the Euclidean space GMM.

In our previous work, we proposed a hierarchical collaborative path planning algorithm based on workspace decomposition [21], demonstrating its advantages in harvesting tasks. The core concept is that the discrete workspace guides the search direction in the continuous configuration space, while progress in the configuration space is fed back to the discrete layer to update the growth direction. In this paper, we introduce an empirical data collection method based on hierarchical collaborative path exploration, incorporating the reward function from Section 3.2 into the collection process.

First, we decompose the workspace of task scene $G_i$ into cubic local regions (called cell) of uniform size $(R_1,R_2,\cdots ,R_n)$, which serve as nodes, with the spatial proportion of obstacles in each cell used as weights to initialize the roadmap M (Algorithm 1, Line 1–2). The cell containing the end effector’s starting position and the cell containing the operation point k are labeled as $R_{state}$ and $R_{goal}$, respectively. We then apply the globally optimal algorithm Dijkstra [22] to find the desired exploration path in M, connecting $R_{state}$ and $R_{goal}$ resulting in a sequence of cells ${\left[R_i^{\left(j\right)}\right]}_{i=1}^k$, where i denotes the cell marked in M and j denotes the cell in the desired exploration path (Algorithm 1, Line 3–4). To avoid data redundancy, after the primary exploration, we score all cells in ${\left[R_i^{\left(j\right)}\right]}_{i=1}^k$. Only those with scores below a certain threshold (risk cells) are explored further. For the primary exploration, we uniformly sample the end-effector states $T=(p,q)$ in Cartesian space. After sampling, all points within two adjacent cells are connected pairwise, and each path is scored for feasibility (Algorithm 1, Line 5–12). Scoring is based on the following equation:

$$\mathrm{Score}\left(R_k\right)={\displaystyle \frac{exp\left(\alpha ·\mathrm{VAL}\left(R_k\right)\right)·exp\left(\beta ·\mathrm{CONN}\left(R_k\right)\right)}{\gamma ·{\mathrm{OBS}}^2\left(R_k\right)}}$$

(10)

where $OBS\left(R_k\right)$ represents the volume ratio of obstacles within the cell space, $VAL\left(R_k\right)$ is the ratio of sampled points for which an IK solution exists and does not collide with obstacles, and $CONN\left(R_k\right)$ is the ratio of connecting lines between sampled points in adjacent cells that lead to the next voxel with a collision-free path. The constants $\alpha ,\beta$ and $\gamma$ are parameters. Cells with scores below a threshold $\eta$ are labeled as risk cells $R_{\mathrm{risk}}=\left\{R_k|\mathrm{Score}\left(R_k\right)\le \eta \right\}$ (Algorithm 1, Line 13).

We explore the cells in $R_{risk}$ sequentially as follows: for each cell $R_k\in R_{risk}$, we uniformly sample the end-effector states $T=(p,q)$ to obtain the sequence $\{T_1^k,T_2^k,\cdots ,T_n^k\}$. To prevent invalid state data from influencing the regression results of the EM algorithm, we discard invalid states without applying weights. Consequently, the reward function introduced in Section 3.1 eliminates $r_{collision},r_{valid},r_{traj}$. Each state $T_i$ is assigned a weight $w_i^k$ base on $r_{pos}$ and $r_{distance}$, using the following formula:

$$w_i^k=exp\left(-{\eta }_1·{\displaystyle \frac{1}{r_{\mathrm{distance}}}}-{\eta }_2·{\displaystyle \frac{1}{r_{\mathrm{pos}}}}\right)$$

(11)

where ${\eta }_1$ and ${\eta }_2$ are constant coefficients. The weight information is then normalized as follows: $w_i^k={\displaystyle \frac{w_i^k-w_{min}^k}{w_{max}^k-w_{min}^k}}$, resulting in the weighted data ${\mathcal{D}}^k=\left((T_1^k,w_1^k),(T_2^k,w_2^k),\dots ,(T_n^k,w_n^k)\right)$. Further decomposition of ${\mathcal{D}}_p^k=\left((p_1^k,w_1^k),(p_2^k,w_2^k),\dots ,(p_n^k,w_n^k)\right)$ and ${\mathcal{D}}_q^k=\left((q_1^k,w_1^k),(q_2^k,w_2^k),\dots ,(q_n^k,w_n^k)\right)$ is shown in (Algorithm 1, Lines 14–22). Using these data as input, we apply the EM algorithm for regression to obtain $G_p^k$ and $G_q^k$. By completing the exploration of all risk cells, we obtain the sets ${\left[G_p^k\right]}_{k=1}^r$ and ${\left[G_q^k\right]}_{k=1}^r$, where r is the size of $R_{risk}$.

Algorithm 1 Data Collecting Based on Hierarchical Collaborative Path Exploring

Require: Task scene $G_i$, start position $s_0$, operating point k Ensure: $D_p$, $D_q$   1: $\mathcal{R}\leftarrow \mathrm{WorkSpaceDecomposition}(G_i,X)$   2: Initialize roadmap ${\mathcal{M}}_i$ using $\mathcal{R}$   3: $R_{\mathrm{start}},R_{\mathrm{goal}}\leftarrow \mathrm{LocalRegionMapping}(s_0,k)$   4: ${\left\{R_i^{\left(j\right)}\right\}}_{j=1}^J\leftarrow \mathrm{Dijkstra}({\mathcal{M}}_i,R_{\mathrm{start}},R_{\mathrm{goal}})$                        ▹ Preliminary exploration   5: for$R_i^{\left(j\right)}\in {\left\{R_i^{\left(j\right)}\right\}}_{j=1}^J$do   6:    ${\{T_1,\dots ,T_n\}}_j\leftarrow \mathrm{Sampling}\left(R_i^{\left(j\right)}\right)$   7:    for $T\in {\{T_1,\dots ,T_n\}}_j$ do   8:          $\mathrm{IK}\left(T\right)$   9:          $\mathrm{CollisionDetection}\left(T\right)$ 10:    end for 11: end for 12: for$\{R_i^{\left(j\right)},R_i^{(j+1)}\}\in {\left\{R_i^{\left(j\right)}\right\}}_{j=1}^J$do 13:    for $T\in {\{T_1,\dots ,T_n\}}_i^{\left(j\right)},T^{\prime }\in {\{T_1,\dots ,T_n\}}_i^{(j+1)}$ do 14:          $\mathrm{isPathFeasible}(T,T^{\prime })$ 15:    end for 16: end for 17: ${\mathcal{R}}_{\mathrm{risk}}\leftarrow \mathrm{FindRiskRegion}\left({\left\{R_i^{\left(j\right)}\right\}}_{j=1}^k\right)$                         ▹ Explore risk regions 18: Initialize $\mathcal{D}\leftarrow \mathsf{\varnothing }$ 19: for$R\in {\mathcal{R}}_{\mathrm{risk}}$do 20:    $\{T_1,\dots ,T_n\}\leftarrow \mathrm{Sampling}\left(R\right)$ 21:    for $T\in \{T_1,\dots ,T_n\}$ do 22:          if T is valid then 23:                $w_i\leftarrow \mathrm{ComputeWeight}\left(T\right)$ 24:                $\mathcal{D}\leftarrow \mathcal{D}\cup \left\{(T_i,w_i)\right\}$ 25:          end if 26:    end for 27: end for 28: $(D_p,D_q)\leftarrow \mathrm{DataSegmentation}\left(\mathcal{D}\right)$ 29: return$D_p,D_q$

The EM algorithm is widely used for parameter estimation in probabilistic models with hidden variables, performed through the E and M steps. When data have associated weights, these weights must be incorporated into both the E-step and the M-step to reflect the importance of each data point. Since each weight is calculated individually as ${\sum }_{i=1}^n{w}_i^k\ne 1$, the EM algorithm must explicitly introduce the sum of weights $S={\sum }_{i=1}^n{w}_i^k$ and normalize the weights as ${w_i^k}^{\prime }={\displaystyle \frac{w_i^k}{S}}$ to ensure unbiased parameter estimation.

In the E-step, the EM algorithm computes the posterior probabilities of the hidden variables using the following equation:

$${\gamma }_{ik}={\displaystyle \frac{w_i{\pi }_k\mathcal{N}\left(x_i;{\mu }_k,{\sum }_k\right)}{{\sum }_{j=1}^K{\pi }_j\mathcal{N}\left(x_i;{\mu }_j,{\sum }_j\right)}}$$

(12)

In the M-step, the algorithm maximizes the expected likelihood function to update the parameters:

$${\pi }_k={\displaystyle \frac{{\sum }_{i=1}^N{\gamma }_{ik}}{{\sum }_{i=1}^N{w}_i}}=\sum _{i=1}^N{\gamma }_{ik}$$

(13)

$${\mu }_k={\displaystyle \frac{{\sum }_{i=1}^N{\gamma }_{ik}{w}_i{x}_i}{{\sum }_{i=1}^N{\gamma }_{ik}{w}_i}}$$

(14)

$${\Sigma }_k={\displaystyle \frac{{\sum }_{i=1}^N{\gamma }_{ik}{w}_i\left(x_i-{\mu }_k\right){\left(x_i-{\mu }_k\right)}^T}{{\sum }_{i=1}^N{\gamma }_{ik}{w}_i}}+\lambda I$$

(15)

where $\lambda I$ is a small regularization term to prevent covariance singularity.

$D_p$ can be directly regressed to obtain a 3D Gaussian Mixture Model (GMM) $G_p\left(x\right)={\sum }_{k=1}^K{\pi }_k\mathcal{N}\left(x|{\mu }_k,{\Sigma }_k\right)$, where $x\in R^3$. For $D_q$, since quaternion are manifolds composed of unit quaternions ($S^3$), traditional GMMs, which assume data in Euclidean space, cannot be applied directly. Ignoring the manifold structure of quaternions can lead to invalid sampling results. To address this, we use tangent space mapping, which locally linearizes the manifold. This allows us to apply the traditional GMM while preserving the geometric properties by first iteratively computing the mean quaternion as a base point using log-exponential mapping as (16). We then map all quaternions in $D_q$ to the tangent space as (17), where $\theta =arccos(q_{ref}·q_i)$ and $v_i\in R^3$, to obtain $D_v=((v_1,w_1),(v_2,w_2),\cdots ,(v_n,w_n))$. We apply the 3D GMM $G_q\left(v\right)={\sum }_{k=1}^K{\pi }_k\mathcal{N}\left(v|{\mu }_k,{\Sigma }_k\right)$ to the data $D_v$. After obtaining the sample results $v_i$ from $G_q\left(v\right)$, we inversely map them to quaternions $q_i$ using (18):

$$q_{\mathrm{ref}}^{(t+1)}=exp\left({\displaystyle \frac{\sum w_i{log}_{q_{\mathrm{ref}}^{\left(t\right)}}\left(q_i\right)}{\sum w_i}}\right)$$

(16)

$$v_i={log}_{q_{\mathrm{ref}}}\left(q_i\right)={\displaystyle \frac{\theta }{sin\theta }}\left(q_i-q_{\mathrm{ref}}cos\theta \right)$$

(17)

$$q={exp}_{q_{\mathrm{ref}}}\left(v\right)=q_{\mathrm{ref}}cos\left|v\right|+{\displaystyle \frac{v}{\left|v\right|}}sin\left|v\right|$$

(18)

At this stage, we can obtain the risk cells $R_{risk}$ for task G and the empirical sampling models ${\left[G_p^k\right]}_{k=1}^r$ and ${\left[G_q^k\right]}_{k=1}^r$. These are then associated as an experience $E=(G,R_{risk},{\left[G_p^k\right]}_{k=1}^r,{\left[G_q^k\right]}_{k=1}^r)$. By selecting a set of typical complex tasks for exploration as described above, we can construct an experience base $\mathcal{E}B=(E_1,E_2,\cdots )$.

3.4. Policy Reconstruction

The policy reconstruction process $\mathcal{RC}\left({\pi }_{\theta },E^{*},t\right)={\pi }^{*}(a_t\mid s_t)$ indicates that the reconstructed policy ${\pi }^{*}$ is derived by merging the empirical model $E^{*}$ with the original agent’s policy ${\pi }_{\theta }$ at time t. This process consists of three steps (Algorithm 2).

Algorithm 2 Real-Time Policy Reconstruction

Require: Primitive agent’s policy ${\pi }_{\theta }$, experience set $E^{*}=(G^{*},{\mathcal{R}}_{\mathrm{risk}}^{*},G_q^{*},G_a^{*})$, observation $s_t$, threshold $ϵ$ Ensure: Action $a_t$   1: $count\leftarrow 0$   2: $a_{\theta }\leftarrow \mathrm{SampleFrom}{\pi }_{\theta }$   3: $a_{E^{*}}\leftarrow \mathrm{GetActionFrom}{E}^{*}\left(s_t\right)$   4: $w_{a_{\theta }},w_{a_{E^{*}}}\leftarrow \mathrm{ActionEvaluation}(a_{\theta },a_{E^{*}})$                                      ▹ Action fusion   5: $p_{\theta },q_{\theta },p_{E^{*}},q_{E^{*}}\leftarrow \mathrm{ActionDisassembly}(a_{\theta },a_{E^{*}})$   6: $p_{\mathrm{mix}}\leftarrow {\displaystyle \frac{w_{a_{\theta }}{p}_{\theta }+w_{a_{E^{*}}}{p}_{E^{*}}}{w_{a_{\theta }}+w_{a_{E^{*}}}}}$   7: $q_{\mathrm{mix}}\leftarrow \mathrm{SLERP}\left(q_{\theta },q_{E^{*}},\gamma ={\displaystyle \frac{w_{a_{E^{*}}}}{w_{a_{\theta }}+w_{a_{E^{*}}}}}\right)$   8: $a_{\mathrm{mix}}\leftarrow (p_{\mathrm{mix}},q_{\mathrm{mix}})$   9: if$\mathrm{Rejection}=\mathrm{TRUE}$then 10:       if $count>ϵ$ then 11:          if $w_{a\theta }>w_{a{E}^{*}}$ then 12:                $a_t\leftarrow a_{\theta }$ 13:          else 14:                $a_t\leftarrow a_{E^{*}}$ 15:          end if 16:       end if 17:       $w_{a_{\mathrm{mix}}}\leftarrow \mathrm{ActionEvaluation}\left(a_{\mathrm{mix}}\right)$ 18:       if $\mathrm{IK}\left(a_{\mathrm{mix}}\right)$ and $\mathrm{notCollision}\left(a_{\mathrm{mix}}\right)$ and $w_{a_{\mathrm{mix}}}>max(w_{a_{\theta }},w_{a_{E^{*}}})$ then 19:          $a_t\leftarrow a_{\mathrm{mix}}$ 20:       else 21:          Go back to line 2 and $count\leftarrow count+1$ 22:       end if 23: end if 24: return$a_t$

Step 1: The actions $a_{\theta }=(p_{\theta },q_{\theta })$ and $a_{E^{*}}=(p_{E^{*}},q_{E^{*}})$ are generated by ${\pi }_{\theta }$ and $E^{*}$, respectively, based on the current observation (Algorithm 2, Line 1–2). A reward function is then introduced to evaluate these actions. Specifically, $p_{E^{*}}=p_G-p_t,q_{E^{*}}=q_G$ where $p_G,q_G$ are sampled from $G_p^{*}$ and $G_q^{*}$, respectively. It is important to note that the GMM model used for $a_{E^{*}}$ varies depending on which risk region the end effector is, as determined by the current state. If the robot is not in risk region, no policy reconstruction is needed, and ${\pi }^{*}\left(a_t\right|s_t)$ reduces to ${\pi }_{\theta }$. The robot must virtually execute the action in the simulation for evaluation, a process that requires the path to be smoothed as much as possible. For example, after executing the action $a_{\theta }$, we set that the end-effector position is updated as $p_{t+1}=p_t+\lambda ·p_{\theta }$, where $\lambda$ is the step size. The rotational maneuver is performed using Spherical Linear Interpolation (SLERP) to ensure path smoothness. The updated orientation is given by $q_{t+1}=SLERP(q_t,q_{\theta },\gamma ){\displaystyle \frac{sin\left(\right(1-\gamma \left)\alpha \right)}{sin\alpha }}{q}_t+{\displaystyle \frac{sin\left(\gamma \alpha \right)}{sin\alpha }}{q}_{\theta }$, where $\alpha =arccos(q_t·q_{\theta })$ and $\gamma$ is the interpolation parameter. Introducing the reward function described in Section 3.2 to evaluate the actions, we obtain $w_{a_{\theta }}=reward(a_{\theta },s_t)$ and $w_{a_{E^{*}}}=reward(a_{E^{*}},s_t)$.

Step 2: Based on the evaluation results, $a_{\theta }$ and $a_{E^{*}}$ are weighted and fused by using (19), (20) and (21) to obtain the mixed action $a_{mix}$ (Algorithm 2, Line 3–7).

$$a_{mix}=(p_{mixed},q_{mixed})$$

(19)

$$p_{mix}={\displaystyle \frac{w_{a_{\theta }}}{w_{a_{\theta }}+w_{a_{E^{*}}}}}{p}_{\theta }+{\displaystyle \frac{w_{a_{E^{*}}}}{w_{a_{\theta }}+w_{a_{E^{*}}}}}{p}_{E^{*}}$$

(20)

$$q_{mix}=SLERP(q_{\theta },q_{E^{*}},\gamma ={\displaystyle \frac{w_{a_{\theta }}}{w_{a_{\theta }}+w_{a_{E^{*}}}}})$$

(21)

Step 3: Action $a_{mix}$ undergoes rejection sampling, and the result is fed back to Step 1 in real time (Algorithm 2, Line 8–18). We define three constraints as rejection conditions. (1) There exists an IK (Inverse Kinematics) solution to the end-effector state after executing $a_{mix}$. (2) The robot can execute $a_{mix}$ collision-free. (3) $a_{mix}$ is better than both $a_{\theta }$ and $a_{E^{*}}$. $reward(a_{mixed},s_t)>max(reward(a_{\theta },s_t),reward(a_{E^{*}},s_t))$. If all three conditions are satisfied, we accept $a_{mix}$; otherwise, we resample to obtain new $a_{\theta }$ and $a_{E^{*}}$, repeating the fusion process. If the number of rejections exceeds the threshold $ϵ$, this indicates that policy reconstruction does not improve performance in the current state, and the action with the higher reward from $a_{\theta }$ and $a_{E^{*}}$ is returned.

4. Experiments

In this section, we address the following three questions through both simulation and real-world experiments:

1.

Effectiveness of Augment agent: Does our policy reconstruction method for augment agent confer performance advantages? We compare the performance of augment agents—derived from original agents trained with varying numbers of episodes—under complex tasks, and benchmark them against traditional path planning algorithms commonly used in fruit-picking robots.

2.

Efficacy of Experience Retrieving: Given that our method relies on retrieving using an experience base, is the proposed similarity-based experience retrieving method effective, and to what extent do augment agents depend on the experience data? To answer this, we construct experience base of varying sizes and examine the impact of library size on the performance of retrieving method as described in Section 3.1. We further compare the performance of augment agent under different experience similarity metrics.

3.

Sim-to-Real Transferability: Can the proposed method retain its effectiveness and advantages when migrating from simulation to real-world settings? We migrate the top-performing algorithms from our simulation experiments, along with our proposed method, to real-world tasks and evaluate their performance comparatively.

4.1. Effectiveness of Augment Agent

We conducted agent training and policy reconstruction experiments in simulation environment Robot Operating System (ROS) and PyBullet [23]. The training environment is built on the Gym [24] framework, and we employ a customized, lightweight SAC algorithm instead of directly using StableBaseline3. The robot used in the experiments is a Realman RM75, a 7-DOF robotic arm equipped with a depth sensor at the end effector. To construct the dataset, we digitally modeled 600 real-world fruit-picking task scenes, including 400 simple scenes and 200 complex ones. Among these, 300 simple scenes were used for training, while 100 complex scenes contributed to the experience base. The remaining 200 scenes (100 simple and 100 complex) were reserved for testing. All experiments were conducted on a system with an AMD Ryzen 7 9700X 3.80 GHz processor and an NVIDIA 4070 Ti Super GPU with 16 GB of VRAM.

During training, we saved the model every 200 episodes from 0 to 6000 and evaluated its performance across three metrics: success rate in simple tasks, success rate in complex tasks, and success rate in complex tasks after retrieval augment. The results, presented in Figure 4a, indicate that by 6000 training episodes, the success rate in simple tasks stabilizes near 100%, while the success rate in complex scenes reaches 56%, suggesting convergence. Under identical training conditions, the augment agent consistently outperforms the original agent in complex tasks, where it maintains a success rate at least 25% higher when convergence occurs. Notably, around 3200 training epochs, we observed significant performance fluctuations, with success rates dropping to approximately 30% in both simple and complex tasks. However, the augment agent maintained a success rate exceeding 50% in complex tasks, demonstrating its robustness against training instability.

Furthermore, we selected the agent at 5600 episodes as a representative for comparative testing against RRT-Connect, RRT*, AIT* [25], and BIT* [26] from the OMPL library [27]. The results, presented in Figure 4b,c, demonstrate that retrieval augment agent outperforms all other algorithms, achieving a success rate exceeding 70%. The original agent at 5600 episodes exhibits a success rate comparable to AIT* and BIT*, all of which surpass RRT-Connect and RRT*. In terms of path quality, DRL-trained agents significantly outperform sampling-based algorithms, achieving path lengths of approximately 0.2 m—less than half of those generated by other algorithms. Sampling-based algorithms’ reliance on random sampling makes it challenging to sample feasible states in highly cluttered environments within the given time constraint (2 s), also resulting in excessively long paths. In contrast, the trained agent benefits from reward-driven guidance, effectively reducing path length. Nonetheless, AIT* and BIT* leverage their ellipsoid-inspired sampling mechanisms to maintain shorter path lengths compared to RRT-Connect.

4.2. Efficacy of Experience Retrieving

We extended the task datasets by constructing experience bases of varying sizes, ranging from 0 to 500 experiences. Ten test tasks were selected to extract scene features. Similarity is computed to retrieve similar experiences from experience bases of varying sizes, and the retrieved results were averaged for subsequent analysis. We classify experiences with similarity greater than 80% as strong similarity and those with similarity between 50% and 80% as middle similarity. As shown in Figure 5a, when the library contains 150 or more experiences, the ratio of strong similarity experiences converges to approximately 0.1, while that of middle similarity experiences converges to about 0.7. Additionally, the similarity of the most similar experiences stabilizes at up to 0.96, indicating that roughly 150 experiences are sufficient for effective policy reconstruction.

Subsequently, we selected experiences with varying similarity levels to augment the agent trained after 5600 episodes and evaluated their performance in complex tasks. As illustrated in Figure 5b, low-similarity experiences had little effect or even slightly reduced the success rate, whereas a significant performance improvement was observed when the similarity reached approximately 0.5, with stabilization occurring above 0.7. This demonstrates that proposed policy reconstruction method is sensitive to experience quality, and that low-quality experiences may adversely affect decision-making.

4.3. Real World

We experimented in a real fruit-picking environment that we built ourselves, selecting AIT* and BIT*—which demonstrated superior performance in simulation—for comparison with the retrieval augment agent at 5600 episodes. Figure 6 illustrates an experimental example of a successful grasp. The results, summarized in

Table 1. The augment agent achieved the highest performance with seven out of ten successes and considerably shorter path lengths than the other algorithms. Owing to real-world uncertainties, the test results were slightly lower than those observed in simulation.

	Trail	Success Trails	Success Rate	Average Path Length	Average Path Time
BIT*	10	4	0.4	0.524	12.6 s
AIT*	10	3	0.3	0.533	11.3 s
agent	10	7	0.7	0.211	10.5 s

, show that the retrieval augment agent achieved the best performance, successfully completing seven out of ten tasks with an average path length of 0.211 m. In contrast, AIT* and BIT* exhibited similar path lengths, both significantly longer than those achieved by agent. These results highlight the limitations of sampling-based planners compared to DRL-trained agent, which benefits from direct reward-guided optimization. In terms of planning time, the proposed agent significantly outperforms both AIT* and BIT*, even with the additional computational steps introduced by action fusion and rejection sampling. In practice, the average per-step decision time of the augmented agent is only marginally higher than that of the original agent, while the average execution time for successful trajectories generated by the original agent is approximately 9.8 s. Additionally, real-world uncertainties introduce some variability in algorithm performance, leading to a slightly lower success rate than in simulation.

5. Discussion

In this paper, we propose a robot path planning decision framework based on retrieval augment, which optimizes the policy in real time by policy reconstruction during the reasoning process. This framework offers a novel solution for robust robot path planning. We also offer an instantiation for the decision framework. For experience base construction, we introduce a high-quality data collection approach based on hierarchical collaborative exploration, and use the weighted EM algorithm and tangent space mapping to extract essential features from the empirical data. For policy reconstruction, we propose a scheme leveraging action evaluation and rejection sampling. Finally, we demonstrate the effectiveness of the method in simulation and real-world fruit-picking tasks.

Several open questions remain. First, large-scale experience base may impact the retrieval efficiency, presenting challenges to the computational and storage capacities of the robot. Developing a cloud-based brain to establish a unified experience base will be a crucial direction for future work. Such a shared experience infrastructure could facilitate knowledge reuse across different robots, crops, and farming environments, significantly reducing duplicated training effort and supporting sustainable, large-scale agricultural deployment. Second, the action fusion process is highly sensitive to the quality of experience and the accuracy of observations. Inaccurate observations may even lead to policy degradation. More robust action fusion methods need to be explored in future research. Finally, although Hu moments are intuitively adopted in this work to characterize the global distribution of obstacles in the similarity metric, comparisons with alternative feature representations have not been explored, which will be considered as an important direction for future research.