LIO-RRTNav for Cattle Yard Inspection Robots: Prior Map Aided Relocalization and Goal-Oriented, Smooth RRT Path Planning

Abstract

Autonomous navigation for inspection robots in cattle barns critically depends on localization and path-planning algorithms. To address the issues of low navigation accuracy, long planning time, and insufficient trajectory smoothness in barn environments, this study proposes a navigation framework that combines LiDAR–inertial odometry and a rapidly-exploring random tree (LIO-RRTNav). For relocalization, this study develops a Fast_LIO2 with relocalization and pose optimization method (RP-Fast_LIO2). It exploits the geometric structure of the barn to associate the current frame point cloud with historical point clouds to suppress drift. Meanwhile, stable features are extracted from dynamic scenes and registered to the global map using the Iterative Closest Point (ICP) algorithm; the registration result is used as the initial state for an iterated extended Kalman filter (IEKF) to refine pose estimation. For path planning, this study proposes a highly efficient, robust, and smooth rapidly exploring random tree method (HRS-RRT). By incorporating a goal-oriented random sampling strategy and a safety-distance constraint, it enables safe and efficient planning, and the resulting path is further optimized through path pruning and cubic B-spline smoothing based on a spring potential energy model. Simulation test results showed that the HRS-RRT algorithm reduced 22.17%, 75.00%, and 83.09% in terms of path length, planning time and number of iterations, respectively, when compared with the traditional RRT algorithm. The experimental results revealed that RP-Fast_LIO2 algorithm reduced the mean and root mean square error of the absolute position error by 83.21% and 79.89%, when comparing with the traditional Fast_LIO2 algorithm. In navigation experiments conducted in two cattle yards, when the robot operated at speeds of 0.3, 0.5, and 1.0 m/s, the maximum lateral and longitudinal deviations did not exceed 0.13 and 0.08 m, respectively, and the maximum heading error did not exceed 7.24°. The results acquired verified the adaptability of the LIO-RRTNav algorithm in the cattle yard environment, meeting the requirements of cattle yard inspection robot operation.