Adjusting the PWM frequency and duty cycle to achieve the desired angular velocity in differential drive robots

Question

I am practicing C++ and intro to robotics using a simple differential drive robot and a raspberry pi. Also following along the GA Tech course on control of mobile robotics.

The implementation I am trying firstly is the Proportional part of a PID controller for a Go-to-goal behavior robot, where I have a known state of [x , y , theta] and I have a desired [x_desired , y_desired , theta_desired].

Assuming a constant velocity, then the only control signal I can affect is the rotation rate omega, such that:

error = theta_desired - theta;
omega = PID(atan2(sin(e), cos(e))); // where Ki and Kd are zero

Using a wheel encoder and basic odometry, I am able to calculate the distance traveled by each wheel and its rotation rate as follows:

const double& CalculateDistanceTraveled()
{
  distance_traveled_ = (2.0 * M_PI *
            (static_cast<double>(wheel_encoder_.get_total_ticks()) / num_slots_)
            * wheel_radius_m_); // in meters
  return distance_traveled_ ;

}

void CalculateRotationRate()
{
  while (this->is_running_)
    {
      current_distance_ = std::copy(this->CalculateDistanceTraveled());

      // rotate rate = distance / 100 ms * (1000 ms / 1 second) 
      rotation_rate_ = (current_distance_ - prev_distance_) * 10. // in seconds
      prev_distance_ = current_distance_;

      std::this_thread::sleep_for( time_window_ ); // 100 milliseconds
    }
}

Then I can update the robot's position and calculate the required omega as follows:

d_right_ = encoder_r_.CalculateDistanceTraveled();
d_left_  = encoder_l_.CalculateDistanceTraveled();

d_center_ = (d_left + d_right) / 2.0;

//d_baseline is distance between the two wheels
theta = (d_right - d_left) / d_baseline_; 

// update (x,y) position based on distance & orientation
x_ += (d_center * cos(theta_rad_)); // prev heading
y_ += (d_center * sin(theta_rad_));

// update heading
theta_rad += theta;

Now I can calculate the error based on the difference between current state and target state assuming constant velocity and using only propotional gain for now

double e = target.theta_rad_ - current.theta_rad_;
// ensure error is between -Pi and +Pi
e_prime_ = atan2(sin(e), cos(e));
omega_ = kp_ * e_prime_;

// velocity is constant and pre-defined
double w_r = ((2 * v_) + (omega * d_baseline_)) / (2 * wheel_radius_m);

 // velocity is constant and pre-defined
double w_l = ((2 * v_) - (omega * odometry_.d_baseline_)) / (2 * wheel_radius_m);

Now that I have the required w_l and w_r I need to turn them to pwm frequency and duty cycle values.

However, AFAIK there is no such equation (since battery charges deplete, etc, etc) which is why we use encoders in the first place.

So the only way I can think of is to use the CalculateRotationRate function, and continuously update the pwm frequency and duty cycle until I get near the required w_r and w_l (as they also change based on the robot's state changing).

At first thought, continuously checking and updating is probably going to introduce oscillations. I can use PID for this, but it seems like a round-about-way of doing it.

I am wondering if my approach for this last part is correct? To be clear, I am not looking for how to code-up pulse width modulation, but rather how to go from desired w_l to PWM frequency and duty cycle values that will achieve the desired w_l and w_r.

Answer 1

At first, I did not go trough your code to check for errors in the formulas but from a high level perspective this seems ok. Therefore, your position controller is fine.

What you lack is a lowlevel controller for the PWM signal. This controller should take the error of w_l - e.g. e_w_l - and w_r and provide a duty cycle accordingly.

For that you should use a PID as well. The resulting system will be a cascaded control loop where the outer (slower) loop corresponds to the position control and the inner loop to the PWM controller (fast).

\begin{align} \text{dutyCycle}_{w} = K_p \cdot e_{w} + K_i \sum_{n=0}^{t} e_w (t) \cdot \Delta t + K_d \frac{e_{w}-e_{w,prev}}{\Delta t} \end{align}

I would start with only a P controller.

INFO: If you desicide to use a I part as well, make sure you also use a Anti-Reset-Windup since the output is strictly limited to 100% dutycycle.

INFO: In order to calculate e_w_{r,l}- or also want to use a D-gain - you need the most recent and the previous measurement. The acquisition of the timestamp is in most cases not super accurate neither deterministic. Therefore, your errors - or D-Part - will become very noisy. This might also be the source for oscillations. A lowpass filter might help you out at that point, when you can live with the delay it introduces.

INFO: Always tune the fast loop - i.e. PWM contoller - first!