机耕道自动驾驶农机局部路径规划

针对机耕道场景下自动驾驶农机行驶的安全性、平稳性与规划实时性的实际需求，该研究提出了一种基于二次规划的局部路径规划方法。首先基于有限状态机构建农机机耕道行驶模式，其次采用横纵向解耦的方法，通过改进状态栅格法分别对农机速度行为和轨迹行为进行决策，随后利用二次规划方法生成满足多目标、多约束条件的农机轨迹和速度，得到最优路径，最后在多种行驶环境中进行仿真和实车试验，行驶参考速度为2 m/s。实车试验结果表明，在绕行静态障碍物场景中，规划轨迹的平均绝对曲率为0.021 m^-1，最大绝对曲率为0.056 m^-1，平均绝对横向误差为3.23 cm，最大绝对横向误差为8.69 cm，农机与障碍物外轮廓的距离大于0.76 m；在规避相向行驶、同向行驶和横穿机耕道的动态障碍物场景中，规划速度的平均绝对速度误差为0.08～0.12 m/s，绝对速度误差小于0.38 m/s，加速度变化范围为-0.38～0.44 m/s²。在规划周期为200 ms的仿真试验中，本文算法平均耗时48 ms，最大耗时75 ms，相比采用静态状态栅格法平均耗时减少38 ms，算法效率提升44%。研究结果可为机耕道场景下的农机局部路径规划提供技术支持。

Local path planning for autonomous agricultural machinery on farm road

Path planning of agricultural machinery is currently focused on static obstacle avoidance infield operation scenarios. The driving environment of the tractor is characterized by semi-structured roads with a width of 3-6 m. The tractor roads are relatively complex without identification lines. Many types of dynamic and static obstacles can be found, including the agricultural machinery temporarily parked on both sides of the road, various agricultural machinery, or other traffic participants traveling in the same or opposite direction, even crossing the tractor road. Large-scale agricultural machinery cannot perform the complex trajectory movements in the narrow and long path. It is highly required for the smoothness of the planned trajectory. At the same time, the speed planning is needed to avoid the dynamic obstacles. In this study, local path planning was proposed using quadratic programming, in order to fully meet the practical needs of safety, smoothness, and real-time planning of autonomous agricultural machinery in the scenario of tractor roads. Firstly, a finite state machine was used to define the driving mode of agricultural machinery for the obstacle categories. Secondly, a horizontal and vertical decoupling was adopted to reduce the redundancy for better timeliness. The behavior decision-making of trajectory and velocity was also designed using an improved dynamic state grid method. Thirdly, the planning starting point was designed with a control preview point to combine the planning and control systems. The discontinuity of the cycle trajectory was then effectively solved during planning. Finally, quadratic programming was used to optimize the sampling path. All planned trajectories, trajectory curvature, and velocities fully met the dynamic constraints. A penalty function of safety distance was introduced to solve the constraints exceeding the feasibility of a quadratic programming solution. That was the domain''s upper limit. A series of vehicle tests were conducted in various driving environments, with a reference speed of 2 m/s. The results show that in the scenario of bypassing static obstacles, the average and maximum absolute curvature of the planned trajectory were 0.021, and 0.056 m^-1, respectively, while the average and maximum absolute lateral error were 3.23, and 8.69 cm, respectively, and the distance from the agricultural machinery to the outer contour of the obstacle was greater than 0.76 m. Furthermore, the average and absolute error of planning speed were 0.08-0.12 m/s, and less than 0.38 m/s, respectively, while the acceleration variation range was -0.38-0.44 m/s, in order to avoid the dynamic obstacles, such as driving in the opposite or same direction, or crossing tractor tracks. Therefore, there was the average and maximum time of 48, and 75 ms, respectively, after simulation with the planning period of 200 ms. The average time was reduced by 38 ms, whereas, the efficiency was improved by 44%, compared with the static state grid method. The findings can also provide technical support to the local path planning of agricultural machinery in the scenario of machine plowing roads.