An Introduction To PID

Self-balancing and Line-following robot

> #PID     #ROS2     #Gazebo

> In this blog, we will first learn about PID. Then using PID, we will implement self-balancing and line-following algorithms on a two wheeled robot. Finally, we will combine the two algorithms to make our bot balance itself and follow a line.

Authors : Aryaman Shardul and Marck Koothoor

Table of Contents:

• PID
  • What is PID
  • Queries regarding PID tuning
• Self balancing algorithm
  • Self balancing code explanation
• Line following algorithm
  • Line following code explanation
• Combining self balancing and line following
  • Combined algorithm code explanation
• Demonstration
• Conclusion
• References

PID

• Trust me, PID (proportional, integral, derivative) control is not as complicated as it sounds, because it is more complicated than it sounds. Just Kidding ;)

What is PID

• PID is the controller algorithm that provides the desired output after summing the individual proportional, integral and derivative terms.


• Proportional Term: Proportional gives an output that is proportional to current error. Actual reading is compared with the set_point and multiplied by proportional gain (Kp). P_term = Kp*error


• Integral Term: Due to the limitation of P_term where there always exists an offset between the actual reading and set_point, I_term provides necessary action to eliminate the steady-state error. It basically adds up the small errors until a limit. I_term = Ki*∫ e(t) dt ; where Ki = Integral gain,∫ e(t) dt = Cumulative error


• Derivative term: I_term cannot predict the sudden change in error, So D_term overcomes this problem as it's output depends on the rate of change of error with respect to time, multiplied by derivative constant.D_term = Kd*(de/dt) ,where Kd = Derivative gain,de/dt = Change in error w.r.t time


• Correction_term: Simply it is, P_term + I_term + D_term

Queries regarding PID tuning

Following are some of the doubts asked by :

1. PatrIck ADams : What sequence to follow in PID ?

       
       P-D-I) Firstly set the Kp term (Kd = Ki = 0), such that the performance is close to our desired state, 
       then set the Kd term (Ki = 0), noticing that perfomance is improved and now set the Ki term and 
       also tweak the limits for cumulative area. After ample of time, if this doesn't seem working for you , 
       then for (P-I-D)
   
   

2. PaIge D'cruz : For self-balancing, the bot isn't lifting up at even maximum forward speed, How to resolve this ?

       
       This can occur if the mass of the bot is comparatively higher than it's wheels , 
       so try reducing the mass.
   
   

3. Parker Ian Dale : P-I-D is working fine for a span of time , then there's a sudden spike, Why?

       
       If you notice P-I-D working fine for a span , do not change the values for Kp,Kd,Ki, I repeat ! 
       Spike can possibly occur due to the increasing cumulative area, so tweak the limits.
       Note : If working with ROS , you can use plotjuggler to tweak values seeing the graph.
   
   

   Replied to the comment: PaIne Daxton : I tried this solution, but it didn't work.

       
       Truely possible , PID works different in different situation. 
       Here are two more reasons and solutions for spike:
       
   
           
   Derivative kick : Any change in set_point causes an instantaneous change in error. 
   The derivative of this change is infinity (in practice, since dt isn’t 0 it just winds up being a really 
   big number).This number gets fed into the pid equation, which results in an undesirable spike in the output.
   Solution for this is that we take the rate of change in inputs directly instead of taking it for the error 
   (where, error = set_point - input;)
   error_rate = (input - last_input); and not error_rate = (error - last_error);
           
           
   Integral kick : When tuning, there might be a drastic change in I_term, 
   this is because suddenly the cumulative area gets halved (Because all of a sudden, 
   you multiply this new Ki times the entire error sum that you have accumulated). 
   Recommended solution for this is to multiply Ki initially itself instead at last.
   I_term += (ki * error);
   
   

4. PatrIcia D'souza : The performance is slightly on either side of the set_point (0), What can I do to attain desired state?

    
       Just change the set_point = 0; to a positive or negative value as per the performance.
   
   

   Self balancing algorithm

• To balance a two-wheeled bot, we give forward and backward velocity simultaneously using PID algorithm.
• We use an IMU sensor plug-in here which gives us the pitch angle readings as input and that helps us in balancing the bot.

Self balancing code explanation

1. Import required libraries and declare the variables.

2. Creating a class for publisher-subscriber node. Subcribe to IMU sensor-plugin /demo/imu and publish velocity to differential drive plugin /cmd/vel

3. Define a function to convert pitch readings from radians to degrees.

4. Define the balacned state of the bot.

5. After calculations for PID, publish the correction speed.

   
               pitch = msg->orientation.y;        // Assigning value of orientation.y taken from imu 
               error = Convert(pitch);           // Converting into degrees 
               
               // Calculations for Pitch error, pitch error difference, previous pitch error, pitch rate, pitch area 
               pitch_error = 5*(error-pitch_desired); 
               pitch_rate = pitch_error - prev_pitch_error; 
               pitch_area += pitch_error ;      
               
               // Resetting pitch_area zero everytime it crosses maximum value 
               if(fabs(pitch_area) > 5.0/1.4) 
               { 
                   pitch_area = 0.0; 
               } 
               
               // Calculating PID terms 
               P_term = Kppitch_error;
               D_term = Kdpitch_rate; 
               I_term = pitch_area*Ki; 
               correction_speed = P_term + D_term + I_term; 
               prev_pitch_error = pitch_error; 
           } 
           
           // Callback function for publisher 
           void timer_callback() 
           { 
               auto message = geometry_msgs::msg::Twist(); 
               message.linear.x = correction_speed; 
               RCLCPP_INFO(this->get_logger(), "Publishing: '%lf'", message.linear.x); publisher_->publish(message); 
           }
     

Line following algorithm

• The bot follows a white line over a black surface.
• For this purpose, we use 'RGB camera sensor' plugin in Gazebo.
• It helps us read the rgb values of the pixels detected by the camera's frame.
• We will use PID for line-following.
• Below is the explanation of the algorithm using code snippets.

Line following code explanation

1. First, we import the various libraries and declare all the variables needed.
2. Then, we create a class and declare the publisher and subscriber nodes. We publish velocity to the differential drive plugin and subscribe to the rgb camera.
3. The callback function of the subscriber first reads the 'r' values of the pixels. We count the number of white pixels and also store the index of the first white pixel.
4. Then we calculate the left deviation or right deviation of the bot from the optimal position and accordingly calculate the error.

   
          if( r > 125.00)
          {
            if(flag == 1)
            {
              index = i+1;                      // Assigning position of first white pixel to variable index
              flag = 0;
            }
             count ++;                          // Counting total number of white pixels
          }
         }
      
          if(count == 10)                       // If we have all white pixels in range of camera
          {
            if(index <= 12)                     // Checking position of first white pixel with respect to ideal position of first white pixel
            {
              error = 16 - index - 4;           // Calculating error by checking for deviation between position of current centre white pixel and ideal centre white pixel if deviation is on the left side
              direction = 1;
            }
            else
            {
              error = index + 4 - 17;           // Calculating error by checking for deviation between position of current centre white pixel and ideal centre white pixel if deviation is on the right side
              direction = 2;
            }
          }
          else if(count >= 5 && count < 10)     // If we don't have all white pixels in range of camera
          {
            if(index < 12)
            {
              error = 12 - index;               // Calculating error by checking for deviation between position of current first white pixel and ideal first white pixel if deviation is on the left side
              direction = 1;
            }
            else if(index > 21)
            {
              error = index - 21;               // Calculating error by checking for deviation between position of current first white pixel and ideal first white pixel if deviation is on the right side
              direction = 2;
            }
          }
   
   

5. Now we calculate the PID terms.

        
       error_area += error;
       error_diff = error - prev_error;
       prev_error = error;
       correction = kp*error + kd*error_diff + ki*error_area;
       ang_speed = correction;
   
   

6. In the callback function of the publisher, we decide the direction in which the bot has to turn depending upon the error calculated above and accordingly provide the angular velocity in the appropriate direction.

     
       if(direction == 1)
       {  
       auto message = geometry_msgs::msg::Twist();
       message.linear.x = 0.2;
       message.angular.z = -ang_speed ;
       publisher_->publish(message);
       }
       else if(direction == 2)
       {
       auto message = geometry_msgs::msg::Twist();
       message.linear.x = 0.2;
       message.angular.z = ang_speed ;
       publisher_->publish(message);
       }
   
   

Combining self balancing and line following

• To combine self-balancing and line-following, we must first ensure that the bot is balanced and only then we will provide it angular velocity.
• The code for self-balancing and line-following remains relatively same. The only major difference would occur in the PID tuning of the bot and a few important conditions to be checked.
• Below is a flow-chart of the algorithm.

Combined algorithm code explanation

1. As said above, the code for self-balancing and line-following remains relatively the same. Here, the only difference comes in line following since we have used a red line instead of white line and have reduced it's thickness. But the overall logic remains same.
2. After writing the self-balancing and line-following codes, we move to the callback function of the publisher.
3. Here, we first check whether the bot is in a balanced state. If not, then we provide it with only linear velcity to balance itself.
4. Once, the bot is balanced, we give it angular velocity too whose direction is decied depending upon the direction of it's deviation from the optimal path.
5. Below is the snippet of this callback funtion of the publihser. ```C++ void timer_callback()

    // If bot is not balanced, provide only linear velocity for balancing else provide both linear and angular velocity 
    { 
        if (balanced_state == false) 
        { auto message = geometry_msgs::msg::Twist(); 
          message.linear.x = correction; 
          message.angular.z = 0.0;
          RCLCPP_INFO(this->get_logger(), "Publishing: '%lf'", message.linear.x); 
          publisher_->publish(message); 
        } 
        else 
        { 
            if (direction == 1) 
            { 
              auto message = geometry_msgs::msg::Twist(); 
              message.linear.x = speed; 
              message.angular.z = -ang_speed; publisher_->publish(message); 
              RCLCPP_INFO(this->get_logger(), "Publishing ang: '%lf;", message.angular.z);
            }
            else if (direction == -1)
            {
            auto message = geometry_msgs::msg::Twist();
            message.linear.x = speed;
            message.angular.z = ang_speed;
            publisher_->publish(message);
            RCLCPP_INFO(this->get_logger(), "Publishing ang: '%lf;", message.angular.z);
            }
        }   
    }

Demonstration

The following video is a demonstration of all the three algorithms implemented on our two wheeled bot.

Conclusion

PID controller is a widely used concept in various projects and real life applications. In this blog, with the help of PID ,we implemented three algorithms on a two wheeled self-balancing + line-following robot. We hope you all enjoyed reading this blog and hope that this blog encourages you all to implement them yourselves. You can have a look at our project's github repository.

References

• For PID - https://www.wescottdesign.com/articles/pid/pidWithoutAPhd.pdf
• For Gazebo Sensor plug-ins - http://gazebosim.org/tutorials?tut=ros_gzplugins#Camera