Autonomous Mobile Bot Part 3: PID Controller for DC Motor

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

Background

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 and 2 of this series.

Part 1: https://juhyungsprojects.blogspot.com/2023/08/autonomous-mobile-bot-part-1-autonomous.html

Part 2: https://juhyungsprojects.blogspot.com/2023/09/autonomous-mobile-bot-part-2-node.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.

Results

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).