Visual Perception and Modelling in Unstructured Orchard for Apple Harvesting Robots

The DASNet is utilised to perform the visual perception task in unstructured orchard environments. DASNet improves the architecture of one-stage network by combining the detection, instance segmentation, and semantic segmentation into a single network model. The detection branch of network utilises a three-level Feature Pyramid Network (FPN) to fuse the information of feature maps from deeper level to the shallower level of the network. In each level of FPN in DASNet model, an Atrous Spatial Pyramid Pooling (ASPP) is applied to encode the multi-scale features of objects into output feature maps. There are two fixed-size anchor boxes which are signed at output branch of each level of FPN (total six anchor boxes in 3-level FPN). Each output branch predicts the class, boundary box, and object mask for each of the objects. The overall design of the detection branch of DASNet is shown in Figure 2 and more details of DASNet model can refer to the reference [21]. DASNet utilises Resnet-50 as backbone since previous of our works suggest that Resnet-50 achieves the balance between performance and computation efficiency.

The semantic segmentation branch is grafted at detection branch of DASNet, which receives the feature maps from C3, C4, and C5 level. To keep consistency of the feature maps from different level of the network, the C5 and C4 level of feature maps are 4$\times$ upsampled and 2$\times$ upsampled to match the size of the feature maps from the C3 level. The fusion between different level is achieved by concatenation operation. The concatenated feature tensor is then 8$\times$ upsampled to form the segmentation results. The detection branch of DASNet outputs the detection and instance segmentation results of fruits while semantic segmentation branch outputs the semantic segmentation results of the branch/trunk. The combination of the detection branch and semantic segmentation branch forms the final output of the DASNet.

Due to the noisy background in unstructured orchard environments, accurate segmentation of branch/trunk is still a challenging task. Therefore, DASNet is expected to segment the major branch/trunk of trees while ignoring the other small branch. A region-connection analysis by using OpenCV API function is applied after DASNet to remove the small region the segmentation results of the branch. Except for the branch, other elements within the orchard environment such can also affect the operation of the manipulator. Thus, the objects which are identified as not belonging to the branch/trunk are also be included in the following environment modelling.

From the environment perception module, the semantic labelling of fruits, branch, and other elements are returned as the form of the image binary mask. Combining the information of depth map from the RGB-D camera, the point clouds are assigned for each class of objects correspondingly. However, the 3D mapping of the environment based on point clouds is not efficient [32]. Point clouds always include many noisy information and details which cannot be used in the path planning and obstacle avoiding. Meanwhile, the large number of points within the RGB-D camera output could also lead to large computation consumption.

OctoMap mapping the 3D environment based on octrees, dividing the space with the number of small-sized voxels. Original OctoMap adopts probabilistic 3D mapping framework to minimise the error in the range measurement due to the moving of objects or robots. In the condition of apple harvesting robots, the current scenario collected by RGB-D camera is considered as in static condition in terms of guiding the following action of the manipulator. Therefore, the binary representation of occupied voxels within octrees is utilised. By setting the minimum size of the voxels, the resolution of the environment mapping can be adjusted based on the situation. In the process of modelling the trunk/branch, the point clouds of each class of objects will be firstly de-noised to minimise the possible measurement error during the sensing. Then, the OctoMap for each class of objects is constructed accordingly based on the given resolution. The workflow of fruit and scenario modelling algorithm is shown in Figure 3.

A 3D-SHT algorithm is developed in this work to estimate the optimal centre position of the fruits from the obtained point clouds of each fruit mask. We assume the geometry of the apple fruit is sphere, and the equation of sphere can be expressed as:

$$(x-c_{x})^{2}+(y-c_{y})^{2}+(z-c_{z})^{2}=r^{2}$$

(1)

$c_{x}$, $c_{y}$, $c_{z}$, and $r$ are the centre and radius of the sphere, respectively. 3D-SHT extend the Circle Hough Transform (CHT) into 3D case, applying vote framework to estimate the most possibly parameters of a sphere which can indicate the actual position and size of detected fruits within scenarios. We firstly spatially uniformed sampling the point clouds of each target to reduce the computational complexity and noise. Then, based on the distribution of point clouds, a searching range of each parameters are calculated accordingly. The searching range of each parameters will be discretized based on given resolution. For each of point within the point clouds, we calculate the corresponding value of radius $r_{est}$ for each possible pair of $c_{x}^{p}$, $c_{y}^{q}$, and $c_{z}^{n}$ within the searching range.

$$r_{est}^{i}=(x_{i}-c_{x}^{p})^{2}+(y_{i}-c_{y}^{q})^{2}+(z_{i}-c_{z}^{n})^{2}$$

(2)

$$if\ r_{est}^{i}\in r_{k}\ while\ r_{k}\in r_{accept},\ Grid\,[c_{x}^{p},c_{y}^{q},c_{z}^{n},r_{k}]+=1$$

(3)

If the calculated value of radius belong to the interval of searching range $r_{accept}$ of acceptable radius, we add one vote on the corresponding parameter pair of $c_{x}^{p}$, $c_{y}^{q}$, $c_{z}^{n}$, and $r_{k}$. Finally, the parameter pair with the highest number of vote generates the estimated sphere of detected fruits.

Based on estimated optimal centre of fruits and distribution of point clouds of each mask, a pose estimation algorithm is utilised to calculate the Euler-angle of each fruits within the scenario. We assume the point clouds (number of points is $n$) which is identified belong to a fruits is the visualised or unblocked partition of the fruits from the current view-angle of the RGB-D camera. According to the pose of each points within the point clouds based on the optimal centre of detected fruits, the optimal access pose of the manipulator to this fruits can be estimated. The parametric equation of a sphere is expressed as follow:

$$\left\{\begin{aligned} &x=Rcos\theta sin\varphi,\\ &y=Rsin\theta sin\varphi,\\ &z=Rcos\varphi.\\ \end{aligned}\right.$$

(4)

In Eq 4, $R$ equals to $\sqrt{x^{2}+y^{2}+z^{2}}$, and $\theta$ and $\varphi$ is in the range of [0,2$\pi$]. We calculate angle $\theta_{i}$ and $\varphi_{i}$ of point $p_{i}$ which belongs to the point clouds, to estimate the unblock pose of fruits under the current view-angle of the RGB-D camera. For chosen point $p_{i}(x_{i},y_{i},z_{i})$ and centre of estimated sphere $C(c_{x},c_{y},c_{z})$ of a fruit mask, we have:

$$p_{c}^{i}=(x_{c}^{i},y_{c}^{i},z_{c}^{i})=(x_{i}-c_{x},y_{i}-c_{y},z_{i}-c_{z})$$

(5)

Then, the angle $\theta_{i}$ can be calculated through the function shown as follow:

$$\theta_{i}=Atan2(\frac{y_{c}^{i}}{R_{xy}^{i}},\frac{x_{c}^{i}}{R_{xy}^{i}}),\ R_{xy}^{i}=R^{i}sin\varphi_{i}=\sqrt{(x_{c}^{i})^{2}+(y_{c}^{i})^{2}}$$

(6)

Similarly, the angle $\varphi_{i}$ can be calculated through the function:

$$\varphi_{i}=Atan2(\frac{z_{c}^{i}}{R_{xy}^{i}},\frac{R_{xy}^{i}}{R^{i}}),\ R^{i}=\sqrt{(x_{c}^{i})^{2}+(y_{c}^{i})^{2}+(z_{c}^{i})^{2}}$$

(7)

The pose of a fruits can be modelling as the ZYX-Euler angle rotation of the coordinate around the centre of the sphere. $\theta$ and $\varphi$ are the rotation angle along the Z-axis and Y-axis, respectively. The rotation matrix therefore can be expressed as:

$$R_{pose}=R_{z}(\theta)\cdot R_{y}(\varphi)\cdot R_{x}(0)={\left[\begin{array}[]{ccc}cos\theta cos\varphi&-sin\theta&cos\theta sin\varphi\\ sin\theta cos\varphi&cos\theta&sin\theta sin\varphi\\ -sin\varphi&0&cos\varphi\end{array}\right]}$$

(8)

While the $\theta$ and $\varphi$ are expressed as follow:

	$\displaystyle\theta=\frac{1}{n}\sum_{i}^{n}\theta_{i},$		(9)
	$\displaystyle\varphi=\frac{1}{n}\sum_{i}^{n}\varphi_{i}$		(10)

To reduce the false prediction of fruit pose due to measurement error of point clouds, we limit the range of estimated angle of $\theta$ and $\varphi$ in the range of [$-\frac{1}{3}\pi$,$\frac{1}{3}\pi$].

Due to the complex arrangement of unstructured orchard environment, visual perception and modelling algorithm may affected by some unexpected factors. Therefore, a pose verification algorithm based on 3DVFH+ is utilised to verify the correctness of estimated pose and secure the manipulator during the operation. This method is to calculate the orientation of barrier in the given neighbourhood of a chosen fruit, to ensure that there is no barrier presented in the orientation of the estimated pose.

We firstly initialise a 2-dimensional histogram $H$ to record the orientation angle $\theta$ and $\varphi$ of the barrier (branch or other elements) to the centre of fruits with a given resolution. Then we search the obstacles within neighbourhood range of $r$ (in mm) of the target fruit. That is, for a barrier $B_{i}$ within the range, the orientation angle $\theta_{i}$ and $\varphi_{i}$ to the centre $C$ of fruits are calculated by using Eqs 6 and 7. Then a penalty value $PV_{i}$ is added to the corresponding location at $H\,[\theta_{barrier}^{i},\varphi_{barrier}^{i}]$, which can be expressed as follow:

$$H[\theta_{barrier}^{i},\varphi_{barrier}^{i}]+=PV_{i},\ while\ PV_{i}=\alpha*K_{i}$$

(11)

The $\alpha$ and $K$ are the class term and distance term of barrier penalty which are expressed as follow:

$$\alpha=\left\{\begin{aligned} &1,\ objects\in branch/trunk,\\ &0.5,\ objects\in other\,element,\\ \end{aligned}\right.$$

(12)

$$K_{i}=\frac{1}{log_{\beta}d_{i}},\ while\ d_{i}=\lVert B_{i}-C\rVert$$

(13)

$\beta$ is a constant to adjust the penalty value related to the distance penalty term. We set $\beta$ and neighbourhood range $r$ equals to 50 and 200, respectively. Then, based on the estimated pose $\theta_{pose}$ and $\varphi_{pose}$ of the chosen fruit, the sum penalty value at $H[\theta_{pose},\varphi_{pose}]$ is used to calculate the confidence rate $L$, which is expressed as:

$$L=\frac{1}{1+exp(H[\theta_{pose},\varphi_{pose}])}$$

(14)

Eq 14 map the confidence rate of estimated pose of a target into the range of [0,1]. Therefore, the confidence rate of estimated pose of each fruits can be represented in form of the probability value. Given a threshold $\tau$ to filter the under-estimated target, which is:

$$\left\{\begin{aligned} &can\_pick:\ True,\ L>=\tau\\ &can\_pick:\ False,L<\tau\\ \end{aligned}\right.$$

(15)

We set $\tau$ as 0.6, and the fruits with higher confidence rate will be assigned as priority in the picking sequence. Furthermore, we should notice that Eqs 11 to 15 only consider the geometry constraint in the environment, other constraints such as robotic working space can be further added into this framework, which can be expressed as:

$$H=\sum PV_{constraint}$$

(16)

The evaluation of environment perception module in terms of object detection, instance segmentation, and semantic segmentation are accomplished by using $F_{1}$ score, Mean Intersection of Union (MIoU), respectively. The $F_{1}$ score is used to measure the detection performance of the network by evaluating the value of $Precision$ and $Accuracy$, which are expressed as follow.

$$Precision=\frac{TruePositive(TP)}{TruePositive(TP)+FalsePositive(FP)}$$

(17)

$$Recall=\frac{TP}{TP+FalseNegative(FN)}$$

(18)

$$F_{1}=\frac{2\times\ Precision\times\ Recall}{Precision+Recall}$$

(19)

The MIoU evaluates the quality of segmentation by measuring the rate between intersection and union of two subsets, which are network prediction of segmentation and ground-truth in this case. The expression of MIoU is shown as follow [37]:

$$MIoU=\frac{1}{k}\sum_{i=1}^{k}\frac{p_{ii}}{\sum_{j=1}^{k}p_{ij}+\sum_{j=1}^{k}p_{ji}-p_{ii}}$$

(20)

The $p_{ij}$ and $p_{ji}$ stand the false positive and false negative of the network prediction of segmentation, respectively.

The performance of DASNet model in terms of detection, instance segmentation, and semantic segmentation are compared with YOLO (and YOLO-tiny), faster RCNN, mask RCNN, and FCN-8s, which are shown in table as follow.

The $MIoU_{F}$ and $MIoU_{B}$ in Table 1 stand the accuracy of instance segmentation on fruits and semantic segmentation on branch, respectively. From the experimental results, DASNet improves the detection performance of one-stage network compared to the YOLO from 0.811 to the 0.833, and achieves comparable performance compared to the two-stage detection network faster RCNN and mask RCNN, which are 0.833, 0.834 and 0.838, respectively. In terms of the instance segmentation of detected objects, DASNet also shows a similar accuracy compared to the mask RCNN, which are 0.857 and 0.871, respectively. The potential reason for the higher accuracy of instance segmentation on mask RCNN is that the two-stage detection network can apply RPN and ROI re-alignment to segment the corresponding area of the objects accurately. On another hand, although DASNet applies ASPP to encode multi-scale features, multi-scale features may also introduce noise and lead to a lower accuracy of segmentation. From the following experimental results, DASNet is capable of providing accurate segmentation of fruits. In terms of the semantic segmentation, DASNet achieves similar performance compared to the FCN-8s model, which are 0.802 and 0.767, respectively. However, due to the complex conditions in the implementation of robotic harvesting in the unstructured orchard, to accurately segment the branch/trunk from the noisy background is still a challenging task. Therefore, environment modelling algorithm considers both inputs from the branch/trunk and elements of other objects within the working scenarios. The working scenarios which are processed by the visual perception algorithm are shown in Figure 6. In addition, the comparison of the computation efficiency of different network models is given in Table 2.

The operating range of the vision system is from 0.3m to 0.7m along the X-axis of the robot coordinate (the minimum steady operation distance of Intel RealSense D435 is 0.2m). To evaluate the performance of the developed system, we extend the maximum test range of visual system up to 1.2m (in the range of ¿0.9m). Table 3 shows the experimental result on 3D-SHT algorithm. Table 5 shows the Average Size of Boundary Box (ASoBBx), Average Number of Pixels (ANoP), and Average Number of Computation Candidate (ANoCC) of each object within images along the distance.

Rather than utilising all point candidates of an object to compute the geometry properties which is computation inefficient and time-consuming, a voxel downsampling algorithm is utilised. This step generates the computation candidates of each object, which lead to more efficient computation. Therefore, ANoCC records the average number of points which are used to calculate for each of the objects in a different range of operating distance.

From the experiment results shown in Table 3, it can be seen that the accuracy of the 3D-SHT algorithm shows a decrease with increasing of the distance from the depth camera to objects. Also, the fluctuating range of estimated parameters of objects becomes larger with the increase of distance. The reason that leads to the results is due to the changing of object scale in different distance within images. As shown in Table 5, with the increase of distance from depth camera to the objects, the ASoBBX, ANoP, and ANoCC show a dramatic decrease. The limited number of point candidates lack the capability of describing the objects with enough information and details, which may lead to false prediction. From the experiment results (Table 3) within the distance of 0.7m, centre estimation algorithm can robustly perform the task with acceptable fluctuating of estimation (around 5.5mm). The accuracy of centre estimation algorithm in the range of 0.3-0.5m and 0.5-0.7m are 0.955 and 0.925, respectively.

The experimental results of the fruit pose estimation algorithm of the objects are shown in Table 4. Similar to the case of centre estimation of fruits pose estimation algorithm can work robustly under the range of 0.7m. The accuracy and SD of $\theta$ and $\varphi$ between 0.3-0.5m and 0.5-0.7m are 0.923, 5.2^∘, 4.9^∘ and 0.885, 8.6^∘ and 7.6^∘, respectively. With the increase of distance from the depth camera (decrease of ANoCC), the accuracy and robustness of pose estimation show a significant decrease. Therefore, a 3DVFH+ based pose verification algorithm is utilised to check the correctness of the estimated pose, which can ensure the safety of manipulator.

Modelling of fruits which are partially blocked by other objects is an important task in the robotic harvesting. With the advance of deep-learning based instance segmentation method, DASNet can robustly perform instance segmentation on such partially Blocked Objects. This experiment evaluates the centre and pose estimation algorithm on fruit in partially blocked conditions. As shown in Figures 4, 7, and 8, the fruit modelling algorithm can efficiently perform the centre localisation and pose estimation when partially surface of fruits are blocked by other elements within the environment. To avoid the failure of modelling algorithm when severely blocked fruits are presented, the fruits without the sufficient number of point candidate will be removed from modelling list. In addition, other factors such as sensor measurement noise could also heavily affect the results of vision processing. The performance of the developed vision algorithm and correspond offset solutions to deal with the variance within orchard are included in the following section.

In the unstructured orchard environment, the depth camera can be affected by the complex arrangement of elements within the scenarios, such as inaccurate depth sensing of points due to the mismatch between the RGB image and the depth image. In addition, the depth-sensing of the objects can be affected by the adjacent objects. Such defect could severely affect the working of the centre localisation and pose estimation algorithm (as shown in Figure 9). Therefore, the point cloud of objects will firstly be de-noised before fruit modelling. That is, the points of an object will be classified as inlier or outlier based on Euclidean distance. Then the points which are classified as outliers will be deleted from the point list. Moreover, based on the size of the object boundary boxes, the objects without the sufficient number of points or with severely in-balance length in different axis (X, Y, Z) will be deleted from object list (as shown in (b) and (c) in Figure 9). In the recent work of robotic harvesting of strawberry [14], similar issues of the depth sensing are reported. Their work applied a Density-Based Spatial Clustering (DBSC) to process the point clouds. Our implementation of point cloud denoising is more concise and computation efficient. Meanwhile, experiment results also indicate that our method is efficient in the case of robotic harvesting in unstructured orchards.

The experiments are conducted in the apple orchard located at Qingdao, China. The plant setting of the orchard is shown in Figure 5. The time period of the experiment is in the range of 10:00 to 16:00 of days in the apple harvest season. As shown the experiment results of processing scenarios within apple orchard in Figure 10, the developed visual perception and modelling algorithm can efficiently sensing the fruit and branch from the RGB image and modelling the 3D working scenario from the depth map.