PID Controller for DC Motor

-

Background

This is part of a series of posts where I document my progress in building an autonomous mobile bot from scratch. The ultimate goal of this project is to build a robot that can accomplish the following tasks:
  • Autonomously map an indoor space using SLAM and save the generated map
  • Load the generated map file and localize the robot inside the map
  • Move the robot around by sending move commands from a GUI that shows the map
During the first few months of the project, I developed an autonomy stack for the mobile robot in a simulated environment and I documented part of the process in part 1(https://juhyungsprojects.blogspot.com/2023/08/autonomous-mobile-bot-part-1-autonomous.html)

The simulated environment consisted of an indoor environment and a two-wheeled robot inside Gazebo, which is a simulation software able accurately replicate real-world physics. The robot sensors (LiDAR, IMU, and wheel encoders) were also simulated using Gazebo sensor plugins. Within the simulation, the autonomy stack successfully mapped the entire indoor space on its own.

With the simulation part of the project completed, it was now time to move onto the real world. First, I had to build the two-wheeled robot with the various sensors attached.

I bought an AlphaBot2-Pi(https://www.waveshare.com/wiki/AlphaBot2-Pi) and heavily modified it to turn it into the robot shown above. I swapped the motors that came with the original Alphabot2-Pi with motors that had hall effect encoders (model name: GBMQ-GM12BY20). I also added a small breadboard with an Arduino Nano and an IMU sensor. I will also install a LiDAR sensor on top of the robot later.

I encountered the first real-world problem overlooked by the simulation when I finished building the robot and ran the two DC motors at the same PWM duty cycle. Even though I set the duty cycle to 50% for both motors, I was getting a noticeable difference in the angular velocities of the wheels. The right wheel was spinning slower than the left wheel, which caused the robot to make a slight turn to the right instead of going straight.

This difference was later discovered to be a fault in the hardware. Even though I set the duty cycle to be 50% in the software, the output duty cycle going to the motors were different. The right wheel was receiving lower average voltage than the left wheel, which explains its slower speed.

This is where the PID controller comes in. PID controllers are almost always used when controlling the speed of a motor. This is mainly because the speed of the motor and the PWM duty cycle is not a linear relationship. You cannot expect a motor to spin at 50% its maximum speed simply because you set the duty cycle to 50%.


If there existed an equation that can give you the accurate duty cycle for a desired wheel velocity, it would be very complicated with a lot of variables. It is very difficult to accurately model the PWM duty cycle vs RPM relationship for a motor because it depends on a lot of variable, such as the load applied to the motor, motor temperature, etc.

The essence of the PID controller is to take the difference between the current wheel velocity and the desired wheel velocity (this is often termed error) and use that difference to adjust the value of the duty cycle. 
Block diagram is often used to illustrate how the PID controller operates. In the case for DC motor control, the diagram is interpreted as follows:
  • r(t) - desired wheel velocity
  • u(t) - PWM duty cycle
  • y(t) - actual wheel velocity
  • e(t) - error
  • Plant Process - DC motor
The actual wheel velocity y(t) is subtracted from the desired wheel velocity r(t) to yield the error e(t). The error is then used to compute the PWM duty cycle y(t), which is fed into the DC motor to produce a new wheel velocity.

Implementation

Implementation was done in C++ with ROS2 Humble.

Firstly, I made a listener that listens on the /wheel_vels topic to constantly update the global variables wheel_vel_r and wheel_vel_l, which are the actual wheel velocities of the left and right DC motors. 
volatile double wheel_vel_r = 0;
volatile double wheel_vel_l = 0;


auto wheel_vels_listener = node->create_subscription<std_msgs::msg::Float64MultiArray>(
    "wheel_vels", rclcpp::SensorDataQoS(), std::bind(&wheelVelsCallback, _1), wheel_vels_listener_options
);


void wheelVelsCallback(const std_msgs::msg::Float64MultiArray& msg)
{
    wheel_vel_r = msg.data[0];
    wheel_vel_l = msg.data[1];
}

This data is transferred from the Arduino Nano to the Raspberry Pi over serial communication and there is a ROS node dedicated to reading from serial and publishing the data to on the topic /wheel_vels.

While the wheel velocity variables are getting updated, the PID controller algorithm executes whenever a message is received over the /cmd_vel topic

The first step in the algorithm is to compute the duration of time since the last iteration of the PID controller 
auto curr_time = std::chrono::high_resolution_clock::now();
double dt = std::chrono::duration_cast<std::chrono::duration<double>>(curr_time - prev_time).count();
prev_time = curr_time;

This value is later used to approximate the derivative and integral.

Then you convert the desired robot velocity received from the /cmd_vel topic to individual wheel velocities 
double target_wheel_vel_r = ((msg.linear.x * 2 / wheel_radius) + (msg.angular.z * wheel_separation / wheel_radius)) / 2;
double target_wheel_vel_l = (msg.linear.x * 2 / wheel_radius) - target_wheel_vel_r;

These equations were obtained from an online source: https://www.roboticsbook.org/S52_diffdrive_actions.html
Although this equation is for obtaining the robot velocity from the wheel velocities, we can manipulate the variables to do the opposite.

Then, the error is calculated and all the terms are inputted into the computeGain() function to obtain the corresponding increase/decrease in the PWM duty cycle. 
double e_r = target_wheel_vel_r - wheel_vel_r;
double e_l = target_wheel_vel_l - wheel_vel_l;

pwm_r += computeGain(dt, e_r, e_int_r, prev_e_r);
pwm_l += computeGain(dt, e_l, e_int_l, prev_e_l);

The computeGain() function is where the Proportion, Integral, and Derivative gains are calculated 
double computeGain(double dt, double e, double& e_int, double& prev_e)
{
    e_int += e * dt;
    double gain = (kp * e) + (kd * (e - prev_e) / dt) + (ki * e_int);
    prev_e = e;
    return gain;
}

In the discrete world of computers, derivative and integrals must be approximated. Integrals are approximated using the Riemann sum and the derivative is approximated by the average slope between current and the previous time point. These approximations become more accurate the smaller the value of dt is.

The gain is then added to the current PWM duty cycle for each wheel. The actual value of the duty cycle is capped from -100 to 100. -100 means the motor should spin backwards with 100% duty cycle.

Result

Both DC motors set to 50% duty cycle (No PID controller)

Robot set to move at vx=0.085 m/s (with PID controller)

Without the PID controller, the robot makes a right turn because the right wheel is turning slower than the left wheel. With the PID controller, the appropriate duty cycle is computed and the robot moves in a straight line.

Apparently, I have to give a higher duty cycle to the right wheel in the code for the wheels to have the same velocity (probably some hardware issue caused by me modifying the Alphabot2-Pi board).

Comments

Post a Comment

Popular posts from this blog

Stereo Visual Odometry (with Bundle Adjustment)

-
Image
github:  https://github.com/Juhyung-L/stereo_visual_odometry Visual odometry is the process of estimating a camera's position by processing the individual images taken from a moving camera. It can be performed with different types of cameras: monocular (single camera), stereo (two cameras), depth (camera with depth info). In this blog, I will explain the inner workings of a visual odometry system for a stereo camera setup. Figure 1: My implementation of stereo visual odometry. A stereo camera is a pair of cameras facing the same direction that are separated by a known distance.  Figure 2: Stereo camera setup. Unlike a monocular camera, the stereo camera can estimate the depth of a pixel on an image using a technique called triangulation. This is similar to how humans can sense how far away an object is using both eyes. The system also includes bundle adjustment, a non-linear error minimization technique used to reduce the accumulation of error in the pose estimates. I hope to ...
Read more

Dynamic Obstacle Avoidance

-
Image
Github:  https://github.com/Juhyung-L/GNC There are several things that go into play when a robot is moving from point A to point B. Assuming the robot has a map of the environment and is localized inside that map, it first needs a GLOBAL path planner. The role of the global path planner is to find the shortest path from the robot's current pose to the goal pose while avoiding obstacle defined by the global map. However, the global path planner alone is not sufficient for moving in a dynamic environment as the global map is static.
Read more

Bird's eye view for wheelchair

-
Image
Github:  https://github.com/Juhyung-L/bird_view Background For my university's capstone project, I was tasked with creating a device that can help our client in visualizing the surrounding environment while driving his power wheelchair. Our client suffered from paralysis below the neck, which meant that his only source of mobility was his power wheelchair. He specifically needed a device that could assist him in visualizing the back half of his wheelchair. Our team took inspiration from the bird's eye view that some car models provide. The working principle behind the bird's eye view is to use multiple wide-angle (fisheye) cameras attached to the sides of the car and computationally combine all video streams to provide a top-down view of the car. Although the system does not look too difficult to implement at first, we quickly realized that making a system that is both durable and ...
Read more