Project
Introduction
This project implements three specialized PID controllers from scratch for autonomous robot navigation, featuring custom control algorithms with advanced rolling window integral terms to prevent windup issues common in traditional implementations. The system employs dual PID control strategies for angular and distance control, enabling precise waypoint navigation with ±1cm positioning accuracy and ±0.001 radian angular precision. The implementation showcases complex maze navigation through 14 waypoints in confined spaces, utilizing a turn-first approach that separates rotational and translational movements for optimal path following. Built on ROS2's multi-threaded executor architecture, the controllers achieve 20Hz real-time control with parallel odometry processing, demonstrating zero steady-state error and minimal overshoot through carefully tuned gains. The rolling window integration technique maintains a 25-sample accumulator that dynamically adjusts the integral term, preventing the saturation issues that plague conventional PID controllers in long-duration navigation tasks.
Objectives
-
To develop custom PID controllers from scratch with rolling window integration preventing integral windup
-
To implement dual control modes separating angular and distance control for precision navigation
-
To achieve sub-centimeter positioning accuracy (±1cm) and millirad angular precision (±0.001 rad)
-
To create complex maze solving capability navigating 14 waypoints through confined spaces
-
To design multi-threaded architecture enabling parallel odometry processing and control execution
-
To demonstrate robust waypoint sequencing with automatic goal advancement and state management
Tools and Technologies
-
Framework: ROS2 Humble with multi-threaded executor
-
Programming Language: C++ 17
-
Control Algorithm: Custom PID with anti-windup rolling window
-
Threading: Callback groups for parallel execution
-
Odometry Processing: Extended Kalman Filter via /odometry/filtered
-
Quaternion Math: Manual Euler conversion using atan2
-
Navigation: Euclidean distance and angular error calculations
-
Communication: Twist messages for velocity commands
-
Build System: Colcon with CMake
-
Dependencies: rclcpp, tf2, geometry_msgs, nav_msgs
-
Version Control: Git
Source Code
-
GitHub Repository: PID Navigation Controllers
-
Documentation: README with implementation details
Video Result
-
Maze Navigation Demo: Complex path execution showing dual PID control through 14 waypoints
-
Control Architecture: Three specialized controllers for distance, angular, and combined maze navigation
-
Performance Metrics: ±1cm position accuracy, ±0.001 rad angular precision, <2s settling time
Process and Development
The project is structured into five critical components: custom PID algorithm implementation, rolling window integration design, dual control strategy development, multi-threaded ROS2 architecture, and complex maze navigation validation.
Task 1: Custom PID Controller Implementation
Core Algorithm Development: Created PID class from scratch implementing p_error = cte (proportional), d_error = cte - p_error (derivative), and windowed i_error accumulation with manual gain application.
Error Calculation Methods: Implemented Euclidean distance error using sqrt((x-xâ‚€)² + (y-yâ‚€)²) for position control and atan2(dy, dx) - yaw for angular error with [-π, π] normalization.
Velocity Command Generation: Developed control output computation as u = Kpp_error + Kii_error + Kd*d_error with saturation limiting to MAX_SPEED = 0.8 m/s for safety
Task 2: Rolling Window Integration Design
Anti-Windup Mechanism: Implemented 25-sample circular buffer (rollingaccumulator) maintaining recent error history, preventing unbounded integral growth during extended operations.
Windowed Accumulation: Created rolling index system where i_error += (new_error - oldest_error), maintaining constant-size integral window that adapts to changing conditions.
Memory Management: Utilized std::vector with fixed size allocation and modulo indexing for efficient O(1) update operations without memory reallocation during runtime.
Task 3: Dual Control Strategy Development
Turn-First Approach: Separated control into two phases: pure angular control (linear.x = 0) to face waypoint, followed by combined control with heading correction during forward motion.
Distance Controller: Implemented pure forward motion PID with single-axis error, achieving straight-line navigation with automatic 3-waypoint sequencing demonstration.
Angular Controller: Created dedicated rotational PID normalizing angle errors to shortest path, handling wraparound at ±π boundaries for optimal turning direction.
Task 4: Multi-Threaded Architecture Implementation
Callback Group Separation: Configured MutuallyExclusive callback groups for odometry subscription (100Hz) and control timer (20Hz) preventing data races in state updates.
Parallel Execution: Utilized MultiThreadedExecutor enabling simultaneous quaternion-to-Euler conversion from odometry while computing PID control outputs without blocking.
Thread-Safe State Management: Protected current_x, current_y, current_yaw updates through separate callback contexts ensuring consistent state during error calculations.
Task 5: Complex Maze Navigation System
14-Waypoint Sequencing: Designed maze solver with alternating turn (z=1.0) and move (z=0.0) waypoints creating smooth navigation through narrow corridors and sharp turns.
Combined PID Control: Implemented weighted combination of angular and distance controllers, with angular correction factor (0.1*angle_error) during forward motion for trajectory tracking
Goal Detection Logic: Created threshold-based goal achievement (0.01m for distance, 0.001 rad for angle) with automatic advancement and completion detection at final waypoint.
Results
The system successfully demonstrates precision autonomous navigation with custom PID control achieving remarkable accuracy across all three controller implementations. The distance controller maintains ±1cm positioning accuracy over sequential waypoints with zero steady-state error through integral action. Angular control achieves ±0.001 radian precision (±0.057 degrees) with fast settling times under 2 seconds per rotation. The maze solver completes complex 14-waypoint navigation in 3.5 minutes, successfully navigating tight corridors and 90-degree turns. Rolling window integration prevents windup during extended operations, maintaining stable control throughout entire missions. The multi-threaded architecture processes odometry at 100Hz while maintaining 20Hz control loops without message drops. Visual demonstrations confirm smooth trajectories without oscillation or jerky movements through proper derivative damping.
Key Insights
-
Rolling Window Superiority: 25-sample windowed integration prevents unbounded error accumulation while maintaining sufficient history for steady-state elimination.
-
Control Separation Benefits: Turn-first strategy simplifies path planning and reduces coupling between angular and linear control loops improving stability.
-
Gain Tuning Criticality: Identical gains (Kp=1.5, Ki=0.007, Kd=0.9) work for both angular and distance control due to normalized error scaling.
-
Threading Necessity: Parallel odometry processing essential for maintaining control frequency as quaternion conversion and atan2 calculations are computationally expensive.
-
Precision vs Speed Tradeoff: Conservative velocity limits (0.8 m/s) enable high accuracy but increase mission duration, highlighting classical control compromise.
Future Work
-
Adaptive Gain Scheduling: Implement velocity-dependent PID gains adjusting control response based on robot speed and turning radius
-
Model Predictive Control: Extend to MPC framework using kinematic models for optimal trajectory generation with constraint handling
-
Path Smoothing Algorithms: Add Bezier curve or cubic spline interpolation between waypoints eliminating sharp transitions
-
Dynamic Obstacle Avoidance: Integrate LiDAR-based reactive navigation maintaining PID control while avoiding moving obstacles
-
Auto-Tuning Implementation: Develop Ziegler-Nichols or relay feedback methods for automatic gain optimization
-
Closed-Loop Path Planning: Combine with A* or RRT* global planners feeding optimal waypoints to PID controllers