# ​Control of Differential Drive Robots

The inspiration for this article is the course “Control of Mobile Robots.” The course is based on the application of modern control theory to the problem of making robots move around in safe and effective ways. The course contained optional Programming Assignments each week which were based on a simulated version (MATLAB) of a Differential Drive Robot called Quickbot. The assignments required implementation of various control strategies to incrementally improve the capabilities of the robot to navigate within the environment to reach its goal while avoiding the obstacles in its path.

### Differential Drive Robot: Quickbot

A differential drive robot is a type of mobile robot whose dynamics are decided by two independently controlled wheels. The robot can be driven based on the relative rate of rotation of its right and left wheels. By controlling the angular velocity of the right and left wheel (vr, vl), the robot’s linear and angular velocity (v, w) can be altered. The robot can be modeled as a unicycle by transforming (v, w) to (vr, vl).

$\frac{dx}{dt} = v\cos(\theta) = \frac{R}{2}(v_r + v_l)\cos(\theta)$
$\frac{dy}{dt} = v\sin(\theta) = \frac{R}{2}(v_r + v_l)\sin(\theta)$
$\frac{d\theta}{dt} = \omega = \frac{R}{L}(v_r - v_l)$

Here R is the wheel radius and L is the distance between the left and right wheels. Using the above transform the robot can be controlled like a unicycle for various tasks like Go-To-Goal and obstacle avoidance.

### Simulated QuickBot

The simulated robot is equipped with 5 IR range sensors to detect obstacles within a distance of 0.3m. Each wheel of the robot has a wheel encoder that increments or decrements a tick counter depending on whether the wheel is moving forward or backward.

### Simulator Basic Component Design

A: Unicycle Model to Differential Model Transform Implementation:  The following MATLAB function is used to transform the controller outputs (unicycle dynamics (v, w)) to differential drive dynamics (vr, vl).

function [vel_r,vel_l] = uni_to_diff(obj,v,w)
L = obj.wheel_base_length;
vel_r = (2*v+w*L)/(2*R);
vel_l = (2*v-w*L)/(2*R);
end

B: Odometry Implementation: This function approximates the location of the robot. The location of the robot is updated based on the difference between the current and the previous wheel encoder ticks. The arc covered by the right and left wheel ($D_r$ and $D_l$) is a function of ticks per revolution of the wheels ($N$). $D_r$ and $D_l$ can be used to calculate the new state of the system as shown below.

$D_r = 2\pi R \frac{\Delta tick_r}{N}$
$D_l = 2\pi R \frac{\Delta tick_l}{N}$
$x_{t+1} = x_{t} + \frac{D_r+D_l}{2} \cos(\theta_t)$
$y_{t+1} = y_{t} + \frac{D_r+D_l}{2} \sin(\theta_t)$
$\theta_{t+1} = \theta_{t} + \frac{D_r-D_l}{L}$

function update_odometry(obj)
% Get wheel encoder ticks from the robot
right_ticks = obj.robot.encoders(1).ticks;
left_ticks = obj.robot.encoders(2).ticks;
% Recal the previous wheel encoder ticks
prev_right_ticks = obj.prev_ticks.right;
prev_left_ticks = obj.prev_ticks.left;
% Previous estimate
[x, y, theta] = obj.state_estimate.unpack();
% Compute odometry here
L = obj.robot.wheel_base_length;
m_per_tick = (2*pi*R)/obj.robot.encoders(1).ticks_per_rev;
r_dtick = right_ticks - prev_right_ticks;
l_dtick = left_ticks - prev_left_ticks;
D_r = m_per_tick*r_dtick;
D_l = m_per_tick*l_dtick;
D_c = (D_l+D_r)/2;
x_dt = D_c*cos(theta);
y_dt = D_c*sin(theta);
theta_dt = (D_r-D_l)/L;
theta_new = theta + theta_dt;
x_new = x + x_dt;
y_new = y + y_dt;
fprintf('Estimated pose (x,y,theta): (%0.3g,%0.3g,%0.3g)\n', x_new, y_new, theta_new);
% Save the wheel encoder ticks for the next estimate
obj.prev_ticks.right = right_ticks;
obj.prev_ticks.left = left_ticks;
% Update your estimate of (x,y,theta)
obj.state_estimate.set_pose([x_new, y_new, theta_new]);
end


C: IR Sensor Voltage to Distance Conversion: The onboard IR range sensors return digital voltage values (between 0.4 to 2.75V) instead of the actual distance values. The following MATLAB function is used to convert these voltage values to the actual distance values. Polynomial interpolation is used to fit voltage-distance (voltage and distance are varying non-linearly) lookup data.

function ir_distances = get_ir_distances(obj)
ir_array_values = obj.ir_array.get_range();
ir_distances_from_table = [0.04 0.05 0.06 0.07 0.08 0.09 0.10 0.12 0.14 0.16 0.18 0.20 0.25 0.30];
ir_voltages_from_table = [2.750, 2.350, 2.050, 1.750, 1.550, 1.400, 1.275, 1.075, 0.925, 0.805, 0.725, 0.650, 0.500, 0.400];
ir_voltages = ir_array_values/500;
coeff = polyfit(ir_voltages_from_table, ir_distances_from_table, 5);
ir_distances = polyval(coeff, ir_voltages);
end

### Go-To-Goal: PID Controller Implementation

The Go-To-Goal task requires the robot to move from its current location to a goal location. In this case, we need to calculate the optimal linear and angular velocity (v, w) to steer the robot towards the goal location. The following function implements a PID controller for that task. In this case, the linear velocity is assumed to be constant and the PID controller controls the angular velocity of the robot by minimizing the error between the goal angle and the robot angle. The angle between goal and robot is calculated by first finding a vector from robot to goal $(u_g = (x_g - x, y_g - y))$.
$\omega = K_P*e_\theta + K_I*\sum{e_\theta} + K_D*\Delta e_\theta$
Here $e_\theta = \theta_g - \theta$

function outputs = execute(obj, robot, state_estimate, inputs, dt)
% Retrieve the (relative) goal location
x_g = inputs.x_g;
y_g = inputs.y_g;
% Get estimate of current pose
[x, y, theta] = state_estimate.unpack();
% Compute the v,w that will get you to the goal
v = inputs.v;
% 1. Calculate the heading (angle) to the goal.
% Distance between goal and robot in x-direction
u_x = x_g - x;
% Distance between goal and robot in y-direction
u_y = y_g - y;
% Angle from robot to goal.
theta_g = atan2(u_y,u_x);
% 2. Calculate the heading error.
theta_e = theta_g - theta; % Error in angle
e_k = atan2(sin(theta_e),cos(theta_e));
% 3. Calculate PID for the steering angle
% Error for the proportional term
e_P = e_k;
% Error for the integral term.
e_I = obj.E_k + e_k*dt;
% Error for the derivative term.
e_D = (e_k - obj.e_k_1)/dt;
% PID Output
w = obj.Kp*e_P + obj.Ki*e_I + obj.Kd*e_D;
% 4. Save errors for next time step
obj.E_k = e_I;
obj.e_k_1 = e_k;
% plot
obj.p.plot_2d_ref(dt, atan2(sin(theta),cos(theta)), theta_g, 'r');
outputs = obj.outputs;  % make a copy of the output struct
outputs.v = v;
outputs.w = w;
end

Robot’s motors have a maximum angular velocity which leads to limitations on the angular velocities of the wheels. So, we need to satisfy this limit. Whenever these limits are violated, we reduce the linear velocity to satisfy the angular velocity control requirements. The following MATLAB function implements this strategy.

function [vel_r, vel_l] = ensure_w(obj, robot, v, w)
% 1. Limit v,w from controller to +/- of their max
w = max(min(w, obj.w_max_v0), -obj.w_max_v0);
v = max(min(v, obj.v_max_w0), -obj.v_max_w0);
% 2. Compute desired vel_r, vel_l needed to ensure w
[vel_r_d, vel_l_d] = obj.robot.dynamics.uni_to_diff(v,w);
% 3. Find the max and min vel_r/vel_l
vel_rl_max = max(vel_r_d, vel_l_d);
vel_rl_min = min(vel_r_d, vel_l_d);
% 4. Shift vel_r and vel_l if they exceed max/min vel
if (vel_rl_max &amp;gt; obj.robot.max_vel)
vel_r = vel_r_d - (vel_rl_max - obj.robot.max_vel);
vel_l = vel_l_d - (vel_rl_max - obj.robot.max_vel);
elseif (vel_rl_min &amp;lt; -obj.robot.max_vel)
vel_r = vel_r_d - (vel_rl_max + obj.robot.max_vel);
vel_l = vel_l_d - (vel_rl_max + obj.robot.max_vel);
else
vel_r = vel_r_d;
vel_l = vel_l_d;
end
% 5. Limit to hardware
[vel_r, vel_l] = obj.robot.limit_speeds(vel_r, vel_l);
end

So, now its time to test the controller performance. For the test [-1,1] coordinate was set as the goal on the map. The linear velocity (v) was set to 0.2 and the robot was initially positioned at $x=0, y=0, \theta=0$. The objective of the PID controller is to move the robot towards the goal.  As you can in the GIF the robot first adjusted its direction towards the heading angle, then moved towards goal and finally stopped near it.

### Obstacle Avoidance: PID Controller Implementation

The obstacle avoidance task requires implementation of various parts of the robot controller for steering it away from the obstacles in its path. The controller makes use of the 5 IR range sensors, by detecting the maximum safe distance readings from these sensors. The distance values returned by the IR sensors are first converted to the robot’s frame of reference using the transformation given below.  Any point at a distance $d_i$ from IR sensor $i$ can be written as vector $u_i = [d_i, 0, 1]'$ from sensor’s frame of reference. This point can be transformed to robot’s frame of reference by multiplying it to transformation matrix $R(x_{s_i},y_{s_i},\theta_{s_i})$.  Here $(x_{s_i},y_{s_i},\theta_{s_i})$ is the pose of the sensor $i$ in robot’s frame of reference. The resulting vector $u'_i$ is transformed to the world’s reference frame by multiplying it to another transformation matrix $R(x,y,\theta)$. Here $(x,y,\theta)$ is the pose of the robot in world’s frame of reference. The resulting vector $u''_i$ is the position of the point from world’s frame of reference.

$R(x_a,y_a,\theta_a) = \begin{bmatrix}\cos(\theta_a) & -\sin(\theta_a) & x_a\\ \sin(\theta_a) & \cos(\theta_a) & y_a\\0 & 0 & 1 \end{bmatrix}$
$u'_i = R(x_{s_i},y_{s_i},\theta_{s_i})\begin{bmatrix}d_i \\ 0 \\1\end{bmatrix}$
$u''_i = R(x,y,\theta)u'_i = R(x,y,\theta)R(x_{s_i},y_{s_i},\theta_{s_i})u_i$

The following MATLAB code implements this transformation.

function ir_distances_wf = apply_sensor_geometry(obj, ir_distances, state_estimate)
n_sensors = numel(ir_distances);
% Apply the transformation to robot frame.
ir_distances_rf = zeros(3,n_sensors);
for i=1:n_sensors
x_s = obj.sensor_placement(1,i);
y_s = obj.sensor_placement(2,i);
theta_s = obj.sensor_placement(3,i);

R = obj.get_transformation_matrix(x_s, y_s, theta_s);
ir_distances_rf(:,i) = R * [ir_distances(i) 0 1]';
end
% Apply the transformation to world frame.
[x,y,theta] = state_estimate.unpack();
R = obj.get_transformation_matrix(x,y,theta);
ir_distances_wf = R * ir_distances_rf;
ir_distances_wf = ir_distances_wf(1:2,:);
end

function R = get_transformation_matrix(obj, x, y, theta)
R = [cos(theta), -sin(theta) x; sin(theta) cos(theta) y; 0 0 1];
end

Now we have to sum up the weighted values of these transformed sensor readings to get a single vector $u_o$. This vector $u_o$ along with robot pose information can be used to find the optimal heading angle ($\theta_h$) to drive the robot away from the obstacle.  This angle is then compared with current robot angle ($\theta$) to get error signal for the PID controller.

$u_o = \sum \alpha_i u''_i$
$\theta_h = atan(u_{oy}, u_{ox})$
$e_{\theta} = atan(sin(\theta_h-\theta), cos(\theta_h-\theta))$
$\omega = K_P*e_\theta + K_I*\sum{e_\theta} + K_D*\Delta e_\theta$

In the case where none of the sensors detect an obstacle, all sensors return maximum distance and the right and left sensor vectors would cancel each other, to steer the robot in the forward direction. If one of the sensors at the right detects an obstacle the resulting right sensor would result in a smaller distance vector. This would result in the resulting vector $u_0$ to direct towards the left of the robot, thus steering the robot towards left (away from the obstacle). The following MATLAB code shows the implementation of the overall PID control strategy.

% Unpack state estimate
[x, y, theta] = state_estimate.unpack();
% Poll the current IR sensor values 1-5
ir_distances = robot.get_ir_distances();
% Interpret the IR sensor measurements geometrically
ir_distances_wf = obj.apply_sensor_geometry(ir_distances, state_estimate);
n_sensors = length(robot.ir_array);
sensor_gains = [1 1 1 1 1];
u_i = zeros(2,5);
u_i(1,:)= ir_distances_wf(1,:) - x;
u_i(2,:)= ir_distances_wf(2,:) - y;
u_ao = sum(u_i,2);
% Compute the heading and error for the PID controller
theta_ao = atan2(u_ao(2), u_ao(1));
e_k = -theta + theta_ao;
e_k = atan2(sin(e_k),cos(e_k));
e_P = e_k;
e_I = obj.E_k + e_k*dt;
e_D = (e_k-obj.e_k_1)/dt;
% PID control on w
v = inputs.v;
w = obj.Kp*e_P + obj.Ki*e_I + obj.Kd*e_D;
% Save errors for next time step
obj.E_k = e_I;
obj.e_k_1 = e_k;


The following GIF shows the simulation result of the above obstacle avoidance PID control.

### Blending the Go-To-Goal and Obstacle Avoidance Behavior

So, far we have developed the go-to-goal behavior and obstacle avoidance behavior based separate controllers. But in the real-world objectives, we would want our robot to reach the goal while avoiding the obstacles in its path. This requires implementing a controller merging together the go-to-goal and obstacle avoidance behavior. In this implementation, we would merge the two behaviors by combining the weighed go-to-goal and obstacle avoidance vectors ($u_g$ and $u_o$) to form a new direction vector ($u_{go}$). Using this vector we would find out the error in current robot angle and implement a PID controller to reduce the error.

$u_{go} = \alpha u_o + (1-\alpha)u_g$
$\theta_h = atan(u_{goy}, u_{gox})$
$e_{\theta} = atan(sin(\theta_h-\theta), cos(\theta_h-\theta))$
$\omega = K_P*e_\theta + K_I*\sum{e_\theta} + K_D*\Delta e_\theta$

% 1. Compute the heading vector for obstacle avoidance
sensor_gains = [1 1 0.5 1 1];
u_i = (ir_distances_wf-repmat([x;y],1,5))*diag(sensor_gains);
u_ao = sum(u_i,2);
% 2. Compute the heading vector for go-to-goal
x_g = inputs.x_g;
y_g = inputs.y_g;
u_gtg = [x_g-x; y_g-y];
% 3. Blend the two vectors
% Normalization
u_ao_n= u_ao/norm(u_ao);
u_gtg_n= u_gtg/norm(u_gtg);
% Blending
alpha= 0.75;
u_ao_gtg = (u_ao_n*alpha) + ((1- alpha)*u_gtg_n);
% 4. Compute the heading and error for the PID controller
theta_ao_gtg = atan2(u_ao_gtg(2),u_ao_gtg(1));&amp;amp;amp;amp;amp;amp;lt;span 				&amp;amp;amp;amp;amp;amp;lt;span id="mce_SELREST_start" style="overflow: hidden; line-height: 0;" data-mce-style="overflow: hidden; line-height: 0;"&amp;amp;amp;amp;amp;amp;gt;&amp;amp;amp;amp;amp;amp;lt;/span&amp;amp;amp;amp;amp;amp;gt;

### Hybrid Automata: Switching Logic Implementation

If the robot is away from the obstacle, using the blended controller (Go-To-Goal + Obstacle Avoidance) would deplete the performance of the robot. So, we need to design hybrid automata to allow the robot to use different controllers depending on the current robot situation. The above image shows the switching logic for the hybrid automata. So, whenever the robot is away from the goal it would use the first go-to-goal PID controller to move in $u_g$ direction towards the goal. Whenever the robot detects an obstacle with its IR sensors it would switch to the blended controller to move in $u_{go}$ direction. Further, if the distance from the obstacle is too low (unsafe) it would switch to the obstacle avoidance controller to move in $u_o$ direction. The following MATLAB code implements the switching logic.

if (obj.check_event('at_obstacle'))
obj.switch_to_state('ao_and_gtg');
end
if (obj.check_event('obstacle_cleared'))
obj.switch_to_state('go_to_goal');
end
if (obj.check_event('at_goal'))
obj.switch_to_state('stop');
end
if (obj.check_event('unsafe'))
obj.switch_to_state('avoid_obstacles');
end

The following GIF shows the simulation result of using the hybrid automata.

Although the above hybrid automata implementation works well in avoiding circular and small obstacles, it is ineffective in avoiding large convex and non-convex (even worse) obstacles. For such cases we need to design a follow wall behavior for the robot, that would enable it to move along the obstacle boundary. This requires an estimation of a vector along the wall boundary. This is done by using 2 neighbor IR  sensors detecting the smallest safe distances. If $p_1$ is the sensor distance vector with smallest IR distance readings and $p_2$ is its neighbor sensor distance vector detecting an obstacle the vector tangent to the obstacle boundary ($u_{fwt}$) can be calculated as difference of $p_1$ and $p_2$ vectors. $u'_{fwt}$ is the normalized vector along the obstacle boundary.  $u_{fwp}$ represents the vector perpendicular to the obstacle boundary from the robot position. $u'_{fwp}$ is the vector pointing towards the obstacle when the distance to the obstacle is more than $d_{fw}$, is near zero when the robot is $d_{fw}$ away from the obstacle, and points away from the obstacle when distance to the obstacle is less than $d_{fw}$.  The net follow wall vector ($u_{fw}$)  is calculated as linear combination of $u'_{fwt}$ and $u'_{fwp}$.

$u_{fwt} = p_2-p_1$
$u'_{fwt} = \frac{u_{fwt}}{\|u_{fwt}\|}$

$u_p = \begin{bmatrix} x \\ y \end{bmatrix}$
$u_a = p_1$
$u_{fwp} = (u_a-u_p)-((u_a-u_p)\cdot u'_{fwt})u'_{fwt}$
$u'_{fwp} = u_{fwp}-d_{fw}\frac{u_{fwp}}{\|u_{fwp}\|}$
$u_{fw} = \alpha u'_{fwt} + \beta u'_{fwp}$

% 1. Select p_2 and p_1, then compute u_fw_t
if(strcmp(inputs.direction,'right'))
direction=-1;
[val, ind] = sort(ir_distances(3:5));
% Pick two of the right sensors based on ir_distances
p_1 = ir_distances_wf(:,min(ind(1), ind(2)));
p_2 = ir_distances_wf(:,max(ind(1), ind(2)));
else
direction=1;
[val, ind] = sort(ir_distances(1:3));
% Pick two of the left sensors based on ir_distances
p_1 = ir_distances_wf(:,min(ind(1), ind(2)));
p_2 = ir_distances_wf(:,max(ind(1), ind(2)));
end
u_fw_t = p_2 - p_1;
% 2. Compute u_a, u_p, and u_fw_tp to compute u_fw_p
u_fw_tp = u_fw_t/norm(u_fw_t);
u_a = p_1;
u_p = [x;y];
u_fw_p = (u_a - u_p) - ((u_a - u_p)'*u_fw_t*u_fw_t);
% 3. Combine u_fw_tp and u_fw_pp into u_fw;
u_fw_pp = u_fw_p - d_fw * (u_fw_p/norm(u_fw_p));
d_llim = 0.15;
d_hlim = 0.25;
if ((val(1)  d_hlim))
u_fw = u_fw_tp .* direction +  u_fw_pp * 10;
else
u_fw = u_fw_tp .* direction +  u_fw_pp;
end

The following GIF shows the simulation result of using the follow-wall behavior.

### Combining it all together

Now we need to merge the follow-wall behavior with our hybrid automata to form a full navigation system for the robot. Whenever the robot is not making progress by switching between go-to-goal behavior and obstacle-avoidance behavior, the robot will switch to follow wall behavior in order to escape the environment. Once the robot has made enough progress it would decide to leave the follow wall behavior. This happens when there is no longer a conflict between the go-to-goal and obstacle avoidance vector. The figure shown below shows the combined hybrid automata.

if (obj.check_event('at_goal'))
if (~obj.is_in_state('stop'))
[x,y,theta] = obj.state_estimate.unpack();
fprintf('stopped at (%0.3f,%0.3f)\n', x, y);
end
obj.switch_to_state('stop');
else
if obj.check_event('at_obstacle')...
&amp;&amp; ~obj.check_event('unsafe')...
&amp;&amp; ~obj.is_in_state('follow_wall')
if sliding_left(obj)
inputs.direction = 'left';
end
if sliding_right(obj)
inputs.direction = 'right';
end
set_progress_point(obj);
obj.switch_to_state('follow_wall');
end
if obj.check_event('unsafe')...
&amp;&amp; ~obj.is_in_state('follow_wall')
obj.switch_to_state('avoid_obstacles');
end
if obj.is_in_state('follow_wall')...
end