Path planning of autonomous agricultural machineries in complex rural road

The Journal of EngineeringVolume 2020, Issue 6 p. 239-245 Research ArticleOpen Access Path planning of autonomous agricultural machineries in complex rural road Jinbo Cui, Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this authorXianfeng Zhang, Corresponding Author cq_zxf@sina.com Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this authorXinmiao Fan, College of Movie and Media, Sichuan Normal University, Chengdu, People's Republic of ChinaSearch for more papers by this authorWei Feng, Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this authorPing Li, Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this authorYu Wu, Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this author Jinbo Cui, Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this authorXianfeng Zhang, Corresponding Author cq_zxf@sina.com Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this authorXinmiao Fan, College of Movie and Media, Sichuan Normal University, Chengdu, People's Republic of ChinaSearch for more papers by this authorWei Feng, Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this authorPing Li, Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this authorYu Wu, Chongqing Academy of Agricultural Sciences, Chongqing, People's Republic of ChinaSearch for more papers by this author First published: 21 May 2020 https://doi.org/10.1049/joe.2019.0840AboutSectionsPDF ToolsRequest permissionExport citationAdd to favoritesTrack citation ShareShare Give accessShare full text accessShare full-text accessPlease review our Terms and Conditions of Use and check box below to share full-text version of article.I have read and accept the Wiley Online Library Terms and Conditions of UseShareable LinkUse the link below to share a full-text version of this article with your friends and colleagues. Learn more.Copy URL Share a linkShare onEmailFacebookTwitterLinked InRedditWechat Abstract This study mainly investigates the path planning and collision avoidance problem of autonomous agricultural machineries in the complex rural road. First, the grid model is used to build the global map of machineries and the particle swarm optimisation algorithm is used to search the global path. A non-linear dynamic inertia weight is introduced to avoid the local lock-up problem of traditional particle swarm optimisation. Then the artificial potential energy method for addressing the local collision problem is used, which is the sum of the attraction of the target point and the repulsion of the obstacles and the road boundary force. To solve the problem of the slip and roll of autonomous machinery caused by complex roads, a machinery dynamic model considering road curvature and topographic inclination is established, and the model predictive control algorithm is used to track the planned path. The simulation results show that proposed method can make the autonomous agricultural machinery driving safely and smoothly. 1 Introduction In recent years, research on autonomous vehicles has become a hot topic in industry and academia, autonomous agricultural machineries are also gradually on the road. Motion planning is a key issue in autonomous agricultural machineries control, which is the process of finding a series of actions that move objects from the initial configuration to the target configuration [1]. Unlike most autonomous vehicles on regular roads, autonomous agricultural machineries mostly drive on winding and uneven rural roads with more obstacles. Therefore, the path planning module of autonomous agricultural machinery has responsibility for determining the optimal path faster and more accurately on the basis of meeting more road and motion constraints so that autonomous agricultural machinery can reach its destination safely and smoothly. Generally speaking, path planning methods can be divided into two kinds: global path planning and local path planning. Global path planning, also called static planning, plan objects' path based on known obstacle information. Common global path planning algorithms include genetic algorithm (GA) [2, 3], rapidly-exploring random tree [4], particle swarm optimisation (PSO) [5] etc. Also, local path planning collects real-time environmental information to get the current location of the map and the distribution of obstacles around the object and then plan the path from the current location to the sub-target node. So this method is also dynamic planning. The commonly used dynamic programming algorithms mainly includes an artificial potential field (APF) [6], A* algorithm [7], and D* algorithm [8]. PSO is a stochastic population-based optimisation technique, and its main advantages are that it is simple to understand, easy to implement, and converges rapidly [1]. However, this algorithm is easy to fall into a local optimal solution, which makes the optimisation effect unsatisfactory. Focusing on these issues, many improved and hybrid algorithms have been proposed. In [9], the PSO algorithm combined with GA applied to spot-welding robots. Li et al. [10] used an estimation of the distribution algorithm to estimate and preserve the distribution information of particles' historical promising best positions to generate three candidate positions for each particle, and then adopt the best candidate position. The APF is capable of treating different obstacles and road structures while planning the optimal path. However, most methods based on PSO are still difficult to deal with path planning in complicated environments due to their high computational cost and local deadlock. In contrast, APF has the advantage of low calculation cost even with complex potential functions for obstacles and road structures. Orozco-Rosas et al. [6] presented a potential field function to determine the desired vehicle yaw angle. Li et al. [11] combined GA and the APF method and proposed the membrane evolutionary APF method to solve mobile robot path planning problems for static and dynamic environments. However, the studies on the potential energy field mostly only consider static obstacles, which is not in line with the actual traffic environment. Moreover, most vehicle control models do not take into account the factors of road inclination and curvature, so they are not suitable for rural roads. The purpose of this study is to propose a path planning algorithm for autonomous agricultural machinery where they can drive safely and smoothly on rural roads with complex obstacles and road structures. First, we build a grid map to provide the global environment including obstacle information for autonomous machinery. Then the dynamic inertia weight is introduced into the traditional PSO to solve the problem of falling into the local optimal solution problem due to the decrease of population diversity in the later iteration period. Then, we establish an APF function including the attraction function considering the location of the mechanical target and the repulsion function considering whether the obstacle is crossable or not and the obstacle velocity. Finally, a path planning controller constraining lateral and longitudinal movement of agricultural machineries is presented. This paper is organised as follows. First, a global path planning algorithm combined with a grid model and PSO and a local crash avoidance based on the potential field are described in Section 2. Second, Section 3 introduces an agricultural machinery dynamics model to constraint the path of machinery and designs a controller to follow the path of machinery. Third, our approach is evaluated through various road simulations, and then the influence of different algorithms is compared and analysed in Section 4. Finally, the main conclusions are given in Section 5. 2 Methods 2.1 Global path planning Grid model is often used in global path planning. Its idea is to divide the movement environment into many grids. Free grids without obstacles are represented in white and obstacle grids with obstacles are represented in black. Agents are only allowed to move in free cells, which can avoid encountering obstacles. To ensure the collision-free movement of agricultural machinery, a global static environment is constructed by using a grid model, as shown in Fig. 1. The black shapes represent obstacles, and the white grids without obstacles are free grids. The actual size of the obstacles is extended into a square, i.e. when the obstacle size is smaller than a grid, treat it as a full grid. The grid at (x, y) is expressed as (1) We employ the PSO [12] method to solve global optimisation problems. Fig. 2 shows the algorithm flow of the global path planning algorithm. First, the workspace is detected to determine whether there are obstacles in the grids. If there are obstacles, judge whether there are nodes. If so, find these advanced nodes. Then the optimal nodes are found and the PSO algorithm is applied among the selected nodes. Finally, repeat the process until the target point. In Fig. 1, green circle represents the starting point, blue circle represents the target point, and red line represents the optimal path. Fig. 1Open in figure viewerPowerPoint Grid map (a) Pre-treatment, (b) Post-treatment Fig. 2Open in figure viewerPowerPoint General structure of the global path planning algorithm In the PSO algorithm, each particle has a fitness function determined by the optimisation function and speed to determine the search direction and speed. Particles are constantly updating their speed and position in the workspace, and the updating process can be described as (2) (3) where and are the velocity and position of the i th particle at the time step t, and express the effect of the cognitive influence and social influence on the particle movement, and and are the random numbers distributed between 0 and 1. is the inertial weight, which can maintain the initial speed and make the particle have the tendency to expand the search space. It plays an important role in the local search capacity and can avoid the model falling into the local deadlock. can be expressed by [5] (4) where and are, respectively, the minimum and maximum inertial weight, is the maximal number of iterations. 2.2 Local collision avoidance Potential field is physics, which is often used to describe the impact of road objects on driving safety. The magnitude and direction of the field intensity vector are determined by road conditions. As shown in Fig. 3, there are usually two kinds of objects on rural roads that affect the safety of agricultural machinery: road boundaries and obstacles. is the limit distance of influence of the obstacle potential field, is the distance from the centre of the obstacle, is the allowed distance between the machinery and the boundary point, and d is the distance from the boundary point. Fig. 3Open in figure viewerPowerPoint Artificial potential field (APF) The total potential field is calculated by the sum of the attraction potential field and the repulsive potential field . The attraction potential field is generated as a function of the current position and the goal position that the machinery must achieve [6] (5) where is the attractive proportional gain. Non-crossable obstacles (e.g. vehicle and pedestrian) and crossable obstacles (e.g. bumped road and small stone) have different effects on machinery. When , obstacles repel machinery to avoid a collision. Rasekhipour et al. [13] used the signed distance between the vehicle and the obstacle to define obstacle potential fields, as shown in (6) and (7). (6) (7) where and are used to describe potential field of the i th non-crossable obstacle and potential field of the j th crossable obstacle, respectively. and are intensity and shape parameters of the i th non-crossable potential field, respectively. and are intensity and shape parameters of the j th crossable potential field, respectively. and are the normalised signed distance between the vehicle and the obstacle. and are the safe longitudinal and lateral distances from the i th non-crossable obstacle, respectively, which are defined as (8) where and are minimum safe longitudinal and lateral distance, respectively. v and u denote the longitudinal velocity and lateral velocity. is lateral velocity of the i th non-crossable obstacle. is the safe time gap, is the constant heading angle between obstacle and machinery. and are distances due to longitudinal velocity difference and lateral velocity difference . and are the safe longitudinal and lateral distances from the j th crossable obstacle calculated similar to (8). and are the forces exerted on agricultural machinery by the road boundary on the left and right sides, respectively. Considering the effect of velocity v, the potential field of the boundary can be given as (9) where and are weight coefficients. The potential field is the sum of the potential fields (10) 3 Machinery dynamics modelling and controller design 3.1 Longitudinal dynamics modelling Fig. 4 shows the longitudinal force of autonomous machinery. Taking into account the tractive force (two terms are present, and correspond to the front wheel and rear wheel, respectively), rolling resistance (contains two items, and correspond to the front wheel and rear wheel, respectively), aerodynamic drag force , and gravity, the force balance along the machinery longitudinal axis yields are expressed as follows : (11) where is the longitudinal velocity, m is the quality of machinery, g is the gravity acceleration, is the road slope angle along with the machinery longitudinal axis yields, and the remaining parameters in (11) are explained by (12)–(16) [14, 15] (12) (13) (14) (15) (16) where and are, respectively, the longitudinal stiffness of the front and rear wheels, is the radius of the wheel, is the longitudinal velocity, and are, respectively, the wheel angular velocity of front and rear wheels, and are rolling resistance coefficients of front and rear wheels, is the adjusting factor of velocity to rolling resistance coefficient, is the mass density of air, is the aerodynamic drag coefficient, and A is the frontal area of the machinery. Fig. 4Open in figure viewerPowerPoint Vehicle longitudinal force diagram 3.2 Lateral motion control Considering the characteristics of rural roads, this study establishes a machinery lateral dynamic model and road slope and road curvature are taken into account. Fig. 5 shows the lateral dynamic model and Fig. 6 depicts a half-car model depicted in [16] machinery steering. The positions of front and rear wheels are represented by and , respectively, and and denote the location of the front and rear wheels in a stationary or inertial coordinate system with basis vectors (). The force balance along the machinery lateral axis yields is given by (17) where is the lateral resultant force on the front wheel, and is the steering angle of the front wheel. is the lateral resultant force on the rear wheel, is the heading of the machinery, and h is the height of the mass centre of machinery. The machinery body roll angle is calculated by (18) where is the yaw moment of inertia, is the road slope angle along the machinery lateral axis yields and are the stiffness and damping coefficient. Fig. 5Open in figure viewerPowerPoint Machinery dynamics modelling based on road inclination Fig. 6Open in figure viewerPowerPoint Schematic diagram of path following model. Black coordinate axis is a global coordinate used to describe displacement, red coordinate axis describes position through basis vector, and orange coordinate axis is a body-fixed axis system used to describes the velocity The front and rear wheels of agricultural machinery must be aligned with wheel orientation to avoid skidding. The constraint equation of front and rear wheels can be described as (19) Combining the geometric relationship between the front wheel and rear wheel, (20) can be obtained (20) The relationship between the position of the rear wheel and the longitudinal speed and heading angle can be described as (21) Combining (20) and (21), the differential constraint of can be given by the distance between the centre of mass of the front wheel and the centre of mass of the rear wheel l(22) The differential constraints of the lateral distance deviation and heading angle deviation between the machinery and the desired trajectory are described as (23) where k is the road curvature. 3.3 Objective functions and restraints Similar to [17], consider a non-linear dynamical system to describe the general optimal motion planning problem. We denote by the state of the vehicle, i.e. (24) where is an input value to control the steering of the machinery and is used to describe road slope and curvature. Then (24) is linearised to facilitate the controller simulation (25) where (26) (27) (28) We choose the lateral velocity, heading, and lateral distance deviation as the output value, , and discretise (25) with the sampling period T(29) where , , Moreover, is an identity matrix with (), k represents the current time step. The objective function consists of three parts: output value , control increment , and relaxation factor , i.e. (30) where and are the prediction horizon and the control horizon, is the reference output value, and , and are weight matrices. The constraints of (30) are as follows (31) where and are the maximum and minimum control increment, respectively. The optimal solution of the controller design, i.e. the control increment sequence of the vehicle system at the current time, is shown in (32) (32) The control model of agricultural machinery can be given by (33) 4 Simulation results This section will evaluate our approach through CarSim/Simulink. The main parameters of the vehicle are: , , , , , . The width of rural roads is 10.5 m. The sampling time of the controller is 0.02 s. To evaluate the stability of autonomous agricultural machineries on rural roads, we will discuss the effects of driving speed, road slope, and road curvature on the driving performances of agricultural machineries, such as yaw velocity, lateral deviation, and centroid inclination. The autonomous machinery runs at 36, 54, and 72 km/h, respectively. The initial position of the autonomous machinery is (0, 1.5). There is a non-crossable static obstacle at (63, 1.5) and (87, 2), respectively. The local path planning and heading of autonomous machinery are depicted in Fig. 7. As shown in Fig. 7 a, although the deviation between the actual path and the target path increases with the increase of the autonomous mechanical speed, these deviations do not exceed 0.3 m. This shows that the controller designed in this study can track the reference trajectory accurately and avoid obstacles. Fig. 7 b depicts the heading of the first obstacle avoidance of the machinery. The heading increases first to avoid obstacles and decreases after obstacle avoidance is completed. Fig. 7Open in figure viewerPowerPoint Simulation results of different speeds (a) Lateral deviation, (b) Heading Autonomous machinery drives at a speed of 36 km/h on roads with slopes of 0°, 10°, and 20°, respectively. The initial position of the autonomous machinery is (0, 6.5) and there is a non-crossable static obstacle at (65, 6.3) that may conflict with machinery. So the machinery needs to turn left to avoid conflict. Fig. 8 illustrates the steering angle of front wheel, the yaw rate, the lateral deviation, and lateral acceleration of the machinery as it evades the obstacle. The steering angle of the front wheel remains at [−3, 0], and changes smoothly. This shows that the output control quantity of the designed autonomous machinery controller varies within the constrained range, and the control increment is small, which can meet the requirements of machinery. The smaller the yaw rate during mechanical steering, the more stable it will be. As shown in Fig. 8 b, with the increase of road slope, the yaw rate of machinery tends to increase. Even so, when autonomous machinery drives on a road with a slope of 20°, its yaw rate does not exceed 2.5°/s. The autonomous machinery has a maximum lateral deviation of 0.07 m when driving on a road with a slope of 0°, a maximum lateral deviation of 0.14 m when driving on a road with a slope of 10°, and a lateral deviation of 0.18 m when driving on a road with a slope of 20°. Moreover, the maximum lateral acceleration is . So, we can conclude that vehicles can drive smoothly on roads with different road inclination angles. Fig. 8Open in figure viewerPowerPoint Simulation results of different road inclinations (a) Steering angle of the front wheel, (b) Lateral deviation, (c) Lateral deviation, (d) Lateral acceleration The lateral deviation, sideslip angle, and yaw rate of the autonomous machinery are shown in Fig. 9 when it travels on the road with a radius of 12, 17, and 22 m at a speed of 36 km/h. As can be seen from Fig. 9, road curvature will affect the stability of autonomous machinery. The lateral deviation ranges of steering on the three road radii are 0–0.27, 0–0.20, and 0–0.12, and their average values are 0.13, 0.08, and 0.05, respectively. Moreover, the side slip angle fluctuation is in a reasonable range and the yaw rate value is small, which reflects the ability of the vehicle to steer smoothly. Fig. 9Open in figure viewerPowerPoint Simulation results of different road curvature (a) Lateral deviation, (b) Side slip angle, (c) Yaw rate In this study, the Rosenbrock algorithm is selected to verify the global optimisation ability of our model. The function can be expressed by (34) The running result of PSO and our algorithm is as shown in Fig. 10. Our algorithm reaches the optimal solution in the 143th iteration, and PSO does not reach the optimal solution until the 500th iteration, which shows that our global search algorithm has faster speed and better accuracy. Setting a grid environment to 10 × 10. The PSO and our algorithm are run 60 times independently, and the results are shown in Table 1. The average fitness value, iteration times, and running time of this algorithm are lower than PSO, which shows that our algorithm can find the optimal path faster. Also, the accuracy of our algorithm is reflected in our shorter path and the higher proportion of the optimal path and the safe path. Fig. 10Open in figure viewerPowerPoint Fitness function Table 1. Results of our algorithm and PSO Performance index Our algorithm PSO average fitness value 6.15 7.24 average iteration times 121 153 average path length 15.3 17.2 average running time 0.54 0.69 optimal path ratio 86.4% 54.2% safety ratio 95.7% 69.4% 5 Conclusion This study presents a path planning algorithm for autonomous agricultural machineries. PSO is used to plan global paths, and APF is used to avoid local conflicts. In addition, we propose a path controller for autonomous machinery based on dynamics. The simulation results show that our method is stable on the road with 10.5 m road width even if the mechanical speed reaches 72 km/h, the road inclination reaches 20°, or the road radius is only 12 m. Also, this method overcomes the deadlock problem of traditional PSO. This study only focused on that agricultural machinery avoids conflict with obstacles at a constant speed and we lack the research on the movement of moving obstacles. In future research, we will improve the existing shortcomings. 6 Acknowledgments This work was supported by three Science and Technology Bureau Project of Chongqing: Capacity Building of Mountain Agricultural Machinery Research and Development Service Center (no. cstc2015ptfw-ggfw70001), Research and Development and Industrialization of Key Techniques and Intelligent Equipment for Rice and Wheat Oil Harvesting in Hilly and Mountainous Areas (no. cstc2018jszx-cyzdX0049), and Design and Key Technologies of Shed Electric Microtiller (no. cstc2018jxj180004). 7 References 1Kim J.J., Lee J.J.: 'Trajectory optimization with particle swarm optimization for manipulator motion planning', IEEE Trans. Ind. Inf., 2015, 11, pp. 620 – 631CrossrefWeb of Science®Google Scholar 2Lamini C., Benhlima S., Elbekri A.: 'Genetic algorithm based approach for autonomous mobile robot path planning', Procedia Comput. Sci., 2018, 127, pp. 180 – 189CrossrefGoogle Scholar 3Jiang A., Yao X., Zhou J.: 'Research on path planning of real-time obstacle avoidance of mechanical arm based on genetic algorithm', J. Eng., 2018, 2018, pp. 1579 – 1586Wiley Online LibraryGoogle Scholar 4Gîrbacia T., Mogan G.: 'An improvement of the rapidly-exploring random tree method for path planning of a car-like robot in virtual environment', Appl. Mech. Mater., 2015, 772, pp. 471 – 476CrossrefGoogle Scholar 5Wang M., Luo J., Yuan J. et al.: 'Coordinated trajectory planning of dual-arm space robot using constrained particle swarm optimization', Acta Astronaut., 2018, 146, pp. 259 – 272CrossrefWeb of Science®Google Scholar 6Orozco-Rosas U., Montiel O., Sepúlveda R.: 'Mobile robot path planning using membrane evolutionary artificial potential field', Appl. Soft Comput., 2019, 77, pp. 236 – 251CrossrefWeb of Science®Google Scholar 7Bing F., Lin C., Zhou Y. et al.: 'An improved A* algorithm for the industrial robot path planning with high success rate and short length', Robot. Auton. Syst., 2018, 106, pp. 26 – 37CrossrefWeb of Science®Google Scholar 8Maurović I., Seder M., Lenac K. et al.: 'Path planning for active slam based on the D* algorithm with negative edge weights', IEEE Trans. Syst. Man Cybernet. Syst., 2018, 48, pp. 1321 – 1331CrossrefWeb of Science®Google Scholar 9Wang X., Shi Y., Ding D. et al.: 'Double global optimum genetic algorithm–particle swarm optimization-based welding robot path planning', Eng. Optim., 2016, 48, p. 18CrossrefWeb of Science®Google Scholar 10Li J., Zhang J.Q., Jiang C.J. et al.: 'Composite particle swarm optimizer with historical memory for function optimization', IEEE Trans. Cybernet., 2015, 45, pp. 1 – 1CrossrefPubMedWeb of Science®Google Scholar 11Li B., Du H., Li W..: 'A potential field approach-based trajectory control for autonomous electric vehicles with in-wheel motors', IEEE Trans. Intell. Transp. Syst., 2017, 18, pp. 2044 – 2055CrossrefWeb of Science®Google Scholar 12Bonyadi M.R., Michalewicz Z.: 'Particle swarm optimization for single objective continuous space problems: a review', Evol. Comput., 2017, 1, pp. 1 – 54CrossrefWeb of Science®Google Scholar 13Rasekhipour Y., Khajepour A., Chen S.K. et al.: 'A potential field-based model predictive path-planning controller for autonomous road vehicles', IEEE Trans. Intell. Transp. Syst., 2016, 99, pp. 1 – 13Google Scholar 14Raffin A., Taragna M., Giorelli M.: 'Adaptive longitudinal control of an autonomous vehicle with an approximate knowledge of its parameters'. Int. Workshop on Robot Motion & Control, Wasowo, Poland, 2017Google Scholar 15Lee H., Choi H.: 'Comparison of fuel efficiency and economical speed for internal combustion engine vehicle and battery electric vehicle using backward-looking simulation', J. Mech. Sci. Technol., 2017, 31, pp. 4499 – 4509CrossrefWeb of Science®Google Scholar 16Paden B., Cap M., Yong S.Z. et al.: 'A survey of motion planning and control techniques for self-driving urban vehicles', IEEE Trans. Intell. Veh., 2016, 1, pp. 33 – 55CrossrefGoogle Scholar 17Cowlagi R.V., Peters S.C., Karaman S. et al.: 'Optimal motion planning with the half-car dynamical model for autonomous high-speed driving'. American Control Conf., Washington, DC, USA., 2013Google Scholar Volume2020, Issue6June 2020Pages 239-245 FiguresReferencesRelatedInformation