1. Introduction
Citrus fruit with rich nutritional value and good taste is loved by people all over the world. According to the statistics of the U.S. Department of Agriculture in April 2020 [
1], China is the second-largest citrus producer in the world. According to the statistical data of the National Bureau of Statistics of China [
2], the annual output of citrus in China shows an increasing trend from 2011 to 2020, and the total output of citrus in 2020 was 5.219 million t. However, at present, citrus fruit is mainly hand-picked, which is inefficient and costly. According to the investigation, the labor force of citrus picking operations accounts for 33–50% of the whole production process. Thus, a picking robot becomes an effective way to alleviate this situation [
3,
4,
5]. Picking robots have long been a research hotspot in the field of intelligent agricultural equipment, and China, the United States, Japan, and other countries have made many breakthrough achievements in this field [
6,
7,
8,
9,
10,
11]. Due to the high operability and precision of the manipulator, most studies have adopted a manipulator as the main execution component of a picking robot.
If a manipulator is used as the main executive part of a citrus-picking robot, the motion planning of the manipulator needs to be solved first. Motion planning refers to the trajectory planning of each joint in the process of moving the manipulator from the initial configuration to the target configuration. During this process, it must be ensured that the manipulator does not collide with any obstacle in the workspace or itself. Existing motion planning algorithms include rapidly random-exploring tree (RRT) [
12,
13,
14,
15], probabilistic roadmap method (PRM) [
16,
17], A* [
18], neural network [
19], and artificial potential field (APF) [
20,
21,
22,
23]. RRT is the most popular algorithm among scientists all over the world to process motion planning problems, and the related algorithms include RRT*, Informed-RRT, RRT-Connect, and so on. RRT gives a path in the workspace by randomly walking and continuously expanding its leaf nodes. This process will not end till the leaf nodes spread throughout the whole workspace, so this algorithm usually takes a long time and is not suitable for a picking robot because of the real-time requirement. Generally, RRT has a good effect on the motion planning problem of mobile vehicles, but it is not feasible to directly use RRT for a manipulator in Cartesian space, because a manipulator cannot be regarded as a single particle. Therefore, some scholars proposed to adopt RRT to carry out motion planning in the configuration space (C-space) of manipulators, in which a point represents a single configuration instance. RRT divides C-space into free and obstructed space and carries out motion planning in the free space. Because a picking robot works in an unstructured and greatly complex environment with many obstacles, the C-space construction of a manipulator would take an extremely long time. From the existing literature, PRM and A* algorithms also need to build C-space, so RRT, PRM, and A* cannot meet the real-time requirements of a picking robot. The neural network algorithm not only needs a long time to train itself, but also has higher requirements on hardware, so it cannot be used for real-time motion planning of a picking robot as well.
APF was firstly proposed by O. Khatib [
23] in 1986 and was applied to real-time dynamic obstacle avoidance motion planning of a manipulator, PUMA560, and a mobile vehicle. APF plans the motion by building a virtual potential field in the workspace which is a Cartesian space, in which moving objects are affected by two kinds of forces—attractive force, and repulsive force—and avoid crashing into obstacles during the process of moving from the initial to the target position. Different from global planning algorithms such as RRT and PRM, APF is a local motion planning algorithm with low algorithm complexity, low dependence on computer resources, and strong real-time performance. However, APF has problems such as local minimum trap (LMT) [
24,
25] and target not reachable problem (TNRP) [
26]. LMT means that when the object has not reached the target position, the gradient of the potential field of APF disappears, then the resultant force becomes zero, and the object in the potential field no longer can reach the target position, which is the main problem and shortcoming of APF. TPNR refers to that when there is an obstacle near the target position, the attraction is far less than the repulsive force, and the object in the potential field cannot overcome the repulsive force to reach the target position under the action of the attractive force. TPNR is caused by the gradual loss of attractive force near the target position which is another main problem and shortcoming of APF. Matoui, F. et al. [
27] designed a non-minimum speed algorithm to improve APF, and the improved APF can effectively avoid the occurrence of the LMT. Jeon, G. et al. [
28] proposed a water sink model for path planning of planar robots aiming at solving LMT. Although the above studies have effectively avoided the LMT, the application object is a mobile vehicle rather than a manipulator, so it cannot be directly applied to a picking robot for real-time motion planning. A large number of scholars have also carried out lots of relevant studies on the application of APF in manipulators. Zhou, H. et al. [
26] improved the repulsive force calculation function of APF and combined it with the cosine adaptive genetic algorithm. Motion planning for the end effector of a 5-DOF manipulator needed 8.72 and 8.31 s respectively in conventional and LMT conditions, and the time was reduced by 51.6% and 53.85% compared with the original APF. Zhang, H. et al. [
20] took 103.421 s to carry out path planning for a dual-manipulator in a static environment based on the potential field of velocity, and the time was 16.85% less than that of APF. Park, S. O. et al. [
29] improved the APF algorithm based on the Jacobian matrix and carried out path planning for a 7-DOF manipulator to avoid spherical and cylindrical obstacles respectively, which both took 7 s, and the single iteration took 1.925 and 2.5 ms respectively. Zhang, L. [
30] combined RRT with APF and proposed a velocity potential field method for obstacle avoidance motion planning of manipulators based on virtual target points. It could effectively avoid falling into LMT, but the path planning for a planar 3-DOF manipulator took at least 4.15 s. Wang, W. et al. [
31] improved the attractive potential field calculation formula of APF by introducing a Cartesian space boundary, but it took more than 5 s for path planning of self-designed 9-DOF manipulator to get very close to the target configuration. Although the above studies successfully applied APF to path planning of manipulators and effectively avoided the emergence of LMT, LMT, and TPNR were not considered at the same time. In addition, due to the high real-time requirements of a picking robot on the motion planning algorithm, the above research results are all time-consuming and cannot meet the real-time requirements of picking robots.
Because of the problems mentioned above, this paper improved the calculation model of both potential fields and forces of APF respectively, and directly calculated the attractive torques in the joint space of a manipulator when planning the motion of a manipulator and proposed a novel improved APF (IAPF) which can effectively reduce the calculation time of motion planning for manipulators and avoid LMT and TPNR problems at the same time. Finally, the correctness and effectiveness of IAPF in manipulators were verified by some illustrative experiments both in the design and realistic picking phase.
The rest of this paper is organized as follows: in
Section 2, a short description of the material and method in this study is given, and the Elementary Transformation Sequence (ETS) method is used to construct the kinematics model of the EC63M manipulator; in
Section 3, the calculation model of potential fields and forces of APF is given, and the method of applying APF on a manipulator is described as well. Furthermore, the optimization strategies in IAPF are introduced carefully. In
Section 4, some illustrative experiments are shown and, in
Section 5 a short conclusion of the whole paper is given.
4. Experiments and Results
All the experiments mentioned in this paper are implemented in the Visual Studio Code through Python 3 in Windows 10 operating system. The hardware conditions are as follows: CPU is I7-9700F, the mainboard is B365M-PiXIU, 16GB memory, and the graphics card is a NVIDIA Quadro P2000.
4.1. Field Surfaces and Force Curves
The potential fields’ surfaces and forces’ curves of both APF and IAPF are shown in
Figure 7 and
Figure 8 respectively.
Table 3 shows the concrete parameters of the potential field and force model of APF and IAPF.
The gradient of APF’s attractive potential field gradually disappears near the target position, while the gradient of IAPF does not disappear but increases. Therefore, IAPF has stronger resistance to TPNR and a faster convergence rate than APF. Due to the phenomenon of gradient explosion in the repulsive potential field of APF, the gradient of the repulsive potential field of APF is much larger than that of the attractive potential field when it is close to an obstacle, resulting in the lack of features of the surface of the attractive potential field of the resultant potential field. However, IAPF can effectively overcome the above problems. Its resultant potential field surface has the common characteristics of both attractive and repulsive potential field surface, which makes IAPF not only have a faster convergence rate but also have the strong obstacle-avoidance ability.
4.2. TPNR Verification
When there are obstacles near the target position, APF has a TPNR problem. Therefore, the following simulation experiment is conducted to verify that IAPF can effectively avoid TPNR. The simulation area is a 10 × 10 rectangle and the concrete parameters are given in
Table 4, and the action threshold of each obstacle is equal. The simulation results are shown in
Figure 9, and the simulation curve is shown in
Figure 10.
As can be seen from
Figure 9, when there is an obstacle near the target position, the object keeps approaching the target position, but is pushed away by the repulsive force of the obstacle and oscillates back and forth, unable to reach the target position by APF. Therefore, APF cannot converge. As can be seen from
Figure 10, IAPF approaches the target position near the 20th iteration and obtains an enhanced attractive force, avoiding the occurrence of TPNR. Simulation results show that the improved attractive force of the IAPF algorithm can effectively avoid the occurrence of TPNR and ensure the success rate of path planning.
4.3. LMT Verification
To verify that the IAPF algorithm can avoid getting into the LMT problem, the following simulation experiment is designed in this section: It is easy to get into the LMT problem when there are obstacles on the line between the target and the initial position. Therefore, APF and IAPF carry out path planning in this case simultaneously. The simulation area is a 10 × 10 rectangle, and the concrete parameters are given in
Table 5 and the action threshold of each obstacle is equal. The simulation results are shown in
Figure 11, and the simulation curve is shown in
Figure 12.
As can be seen from the simulation results, APF quickly falls into the LMT and remains in this state until the path planning fails. IAPF did not fall into the LMT and quickly reached the target position. IAPF reached its target location near iteration 22, while APF remained in the LMT. The simulation results show that IAPF can overcome the LMT problem effectively and ensure the success rate of path planning.
4.4. Application in EC63M Manipulator
In this section, APF and IAPF algorithms were used to carry out path planning for the EC63M manipulator. The experimental parameters were shown in
Table 6, in which the initial configuration was
Θi, the target configuration was
Θg, and the obstacle coordinate was
POi, the obstacle radii were all 120 mm. The simulation results are shown in
Table 7. The joint angle curves and the distance curves between each ETS line segment of the EC63M manipulator and the obstacles are shown in
Figure 13.
The simulation results show that IAPF is better than APF. The joint angles’ curves of IAPF are smoother and the change rate is almost the same. The joint angles’ curves of the APF algorithm change more obviously, and the change rate is quite different. The joint angle θ3 begins to converge after a reverse process, while the joint angle θ2 shows a wave peak. The convergence speed of APF is very slow. Compared with IAPF, it directly reaches the target configuration at the 238th time, but APF converges at the 549th time. Compared with APF, IAPF reduces the operation time by 54.89% and the total joint error by 45.41% in terms of running speed and accuracy.
4.5. Application in Citrus Picking Robot
To further verify the effectiveness and feasibility of IAPF, in this section, the motion planning experiment was carried out on the ICPR and experimental results were shown in
Table 8. The target citrus tree and its simplified model are shown in
Figure 14. Firstly, the target citrus tree was modeled by the partial spherical envelope (PSE) method which was like a bounding sphere algorithm [
36,
37]. PSE method only includes sphere and line segment models, which can greatly reduce the amount of calculation in the process of collision detection, to further ensure that IAPF can meet the real-time requirements of ICPR. PSE envelops all non-current picking parts of citrus trees through spheres and models branches in the current picking part through straight segments. The PSE model of picking three oranges on the top of the target citrus tree is presented in
Figure 15, the picking simulation of the ICPR and the trajectory curves are shown in
Figure 16, the experiment results of IAPF applied to the ICPR are shown in
Figure 17.
IAPF took 0.40 s, 1.12 s, and 1.04 s respectively in the process of path planning for ICPR picking citrus 2 on the top of the target citrus tree, and the total joint errors were 1.33, 1.10, and 1.13 degrees respectively in the stages of picking, releasing, and initialization. ICPR took 2.56 s to plan a path to pick single citrus. Since the time spent on the EC63M mechanical arm’s movement is much longer than the time spent in path planning, the IAPF algorithm can fully plan the next citrus picking path during the ICPR picking process. From the experimental results, it can be concluded that the IAPF algorithm has fast solving speed and accuracy, and can meet the real-time requirements of the ICPR.
5. Conclusions
To improve the efficiency and success rate of the intelligent citrus-picking robot, the motion planning problem of a manipulator—which was the core component of the citrus-picking robot—was studied in-depth, and a novel improved APF algorithm was proposed by improving the models of potential fields and forces of APF and designing some optimization strategies, which greatly reduced the calculation amount of its application in the process of motion planning of a manipulator. In this paper, the ETS method is used to model the kinematics of the EC63M manipulator, and the Jacobian matrix of each point on the manipulator relative to the base coordinate frame can be obtained easily by this model. The improved and original algorithms were both used to carry out the motion of the EC63M manipulator mounted on the intelligent citrus-picking robot. Compared with the original method, the improved one reduced the operation time by 54.89% and the total joint error by 45.41%. Therefore, the improved APF in this paper can not only effectively avoid the problems of the local minimum trap but also the target position not reachable problem existing in APF. Concisely, the improved APF has stronger real-time and accuracy performance. Furthermore, because the working environment of a citrus-picking robot is too complex, this paper used the PSE method to model the target citrus tree. The experimental results show that the PSE method can effectively reduce the calculation amount of motion planning for collision detection. Therefore, the improved APF mentioned in this paper is better than the original algorithm, concerning both real-time performances and the accuracy of the citrus picking robot. This paper presents the PSE method of the target citrus tree but does not explain the general situation for any other citrus tree, which will be the next research goal of our project team. In this experiment, we did not consider the impact on the branch directly attached to the fruit when detaching the citrus fruit from it and the optimal picking orientation of the gripper, and these matters will be part of our forthcoming research as well.