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.