Motion planning of citrus harvesting manipulator based on informed guidance point of configuration space

Abstract: Harvesting robot is the representative of agricultural intellectualization. In the process of citrus harvesting, the manipulator sometimes needs to go deep into the canopy to harvest citrus. Many branches inside the canopy often form a closed polygon channel. Compared with single branch obstacle, the closed polygon obstacle is more difficult to avoid and it takes longer time to plan trajectory for obstacle avoidance. In order to solve this problem, an off-line configuration space mapping method is proposed in this paper, which can reduce the dimension and visualize the high-dimensional configuration space. The first three joints which have great influence on the manipulator are mainly considered, and the impact of the latter three joints on obstacle avoidance also taken into account, thus reducing the information lost in the process of dimension reduction. The visualization of configuration space for crawler chassis and obstacles has certain guiding significance for the later planning algorithm analysis. The topological properties of closed polygonal obstacles in configuration space are analyzed. The projection of closed polygonal obstacles in configuration space can be simplified into upper and lower parts. The upper and lower parts will be partially connected. The middle part is what the cavity is connected with the outside non-collision configuration space, with only two openings. If the end position of the manipulator is located in the cavity (the manipulator extends into the closed polygon), it can only pass through the upper and lower openings. According to this property, the bidirectional fast extended random tree algorithm (RRT-connect) is improved, and an RRT-connect algorithm with informed guidance point (IGPRRT-connect) based on prior knowledge guidance points in configuration space is proposed. The algorithm searches for narrow-channel guidance points in configuration space according to the topological properties of closed polygons and applies the bridge test algorithm to find the correct narrow-channel. Planning from the guiding point to the starting point and the end point respectively greatly speeds up the planning speed of the RRT-connect algorithm in a closed polygon environment. Taking the square box obstacle as an example, the simulation results show that RRT is faster than IGPRRT-connect when the side length is larger than 40 cm, while less than 40 cm, the IGPRRT-connect has advantages as follows: it takes 1.7 s and 1.2 s for RRT-connect and IGPRRT-connect algorithm respectively to motion planning for the obstacle with side length of 35 cm; that of 3.1 s and 1.6 s respectively for side length of 30 cm; when the edge length is reduced to 25 cm, the planning time for RRT-connect algorithm is as high as 18.1 s, while that of IGPRRT-connect is only 2.6 s, which decreases by 86%. At the same time, simulation experiments are carried out under different shapes of obstacles. The results show that IGPRRT-connect algorithm often takes less time than RRT-connect in closed polygon environment. Because IGPRRT-connect algorithm spends a lot of time in searching for non-existent boot configurations in unclosed polygon environment, RRT-connect algorithm performs better than IGPRRT-connect algorithm in unclosed polygon environment. In order to solve the problem of IGPRRT-connect algorithm in unclosed polygon environment, parallel programming for RRT-connect and IGPRRT-connect is carries out in this paper, and two threads are created: one thread runs RRT-connect algorithm and the other thread runs IGPRRT-connect algorithm. When one thread completes the planning, both threads stop completely and output the planned path, thus avoiding the artificial choice of which algorithm to use, which improves the intelligence of the harvesting robot. Parallel programming is beneficial to simplify the program solution: it is not needed to write the algorithm to judge whether RRT-connect or IGPRRT-connect should be used in the current environment. Simulation results show that the parallel algorithm performs well in various environments. Finally, an indoor obstacle avoidance experiment is carried out using parallel algorithm on the prototype of Citrus harvesting robot. In the experiment, the average planning time in the closed polygon obstacle environment is 1.431 s, the successful rate of obstacle avoidance is 88%, while that in the unclosed polygon obstacle environment are 1.064 s and 94%. The experimental results show that the IGPRRT-connect algorithm proposed in this paper has a good obstacle avoidance effect on both closed and unclosed obstacles, which is of great significance to the research of Citrus harvesting robot.