To address issues such as the confusion of environmental feature points and significant pose information errors in chili fields, an autonomous navigation system based on multi-sensor data fusion and an optimized TEB (Timed Elastic Band) algorithm is proposed. The system’s positioning component integrates pose data from the GNSS and the IMU inertial navigation system, and corrects positioning errors caused by the clutter of LiDAR environmental feature points. To solve the problem of local optimization and excessive collision handling in the TEB algorithm during the path planning phase, the weight parameters are optimized based on environmental characteristics, thereby reducing errors in optimal path determination. Furthermore, considering the topographic inclination between rows (5–15°), 10 sets of comparison tests were conducted. The results show that the navigation system reduced the average path length by 0.58 m, shortened the average time consumption by 2.55 s, and decreased the average target position offset by 4.3 cm. In conclusion, the multi-sensor data fusion and optimized TEB algorithm demonstrate significant potential for realizing autonomous navigation in the narrow and complex environment of chili fields.

To address issues such as leaf occlusion, misalignment of the harvesting robotic arm, and limited harvesting range in hillside chili fields, this paper designs an intelligent harvesting system based on 3D point cloud reconstruction and multi-mechanism collaborative leveling. The system integrates real-time data from a LiDAR and IMU inertial navigation system to reconstruct the chili point cloud occluded by leaves from multiple perspectives. To address issues such as misalignment of the robotic arm caused by terrain undulations, the system integrates an adaptive leveling platform and an H-shaped planar slide, combined with a gyroscope to dynamically adjust the arm’s posture in real time, ensuring arm stability while expanding its workspace. In addition, to ensure harvesting efficiency and pepper integrity, an integrated cutting–gripping flexible end effector is designed to achieve synchronized cutting and collection operations. The experiment shows that the system achieves recognition accuracy of 81.95% for occluded chili peppers and 89.04% for non-occluded chili peppers. The harvesting success rate is 86.33%, with a single harvesting operation taking 13.17 s. During prolonged operation, the harvesting success rate can be maintained at approximately 85.1%. In summary, the intelligent harvesting system based on 3D point cloud reconstruction and multi-mechanism collaborative leveling provides a feasible solution for automated pepper harvesting.