Purpose <p>This study introduces an optimized algorithm for autonomous navigation of field robots, aiming to improve navigation accuracy, reduce crop damage and shorten execution times in agricultural environments.</p> Methods <p>The enhanced solution integrates advanced data filtering with sensor fusion techniques, combining LiDAR and IMU inputs to produce precise 3D point cloud representations for reliable navigation in structured crop rows. Both the legacy and improved algorithms were evaluated through simulation and physical trials on the FarmBeast robotic platform.</p> Results <p>The improved algorithm reduced traversal time by up to 33% on certain field sections and lowered crop damage by 25% compared to the previous version.</p> Conclusions <p>Results confirm the robustness and effectiveness of the enhanced navigation system in complex agricultural field conditions, demonstrating its potential for practical deployment within farming automation.</p>