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

                            quickbot_ddr

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

                                    simbot

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)
    R = obj.wheel_radius;
    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
    R = obj.robot.wheel_radius;
    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 > 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 < -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.

L3b.gif

a

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);
% Compute the heading vector
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.

L4a.gif

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));<span 				<span id="mce_SELREST_start" style="overflow: hidden; line-height: 0;" data-mce-style="overflow: hidden; line-height: 0;"></span>

Hybrid Automata: Switching Logic Implementation

hybrid_automata1

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.L5a.gif

Follow Wall Behavior Implementation

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.

L6.gif

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.

hybrid_automata2

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')...
            && ~obj.check_event('unsafe')...
            && ~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')...
        && ~obj.is_in_state('follow_wall')
        obj.switch_to_state('avoid_obstacles');
    end
    if obj.is_in_state('follow_wall')...
            && obj.check_event('progress_made')...
            && ~obj.check_event('unsafe')...
            && ~obj.check_event('at_obstacle')
        obj.switch_to_state('go_to_goal');
    end
end

The GIF below shows the simulation result of the combined system. The obstacle is lot more complex and without proper implementation, the robot can get stuck inside it. The goal is set to [1.1, 1.1] instead of the previous goal of [-1, 1].

L7.gif

Clearly, the robot is able to navigate through the environment to reach the goal by switching between different behavior. At the very start, there is a conflict between go-to-goal and obstacle avoidance vector. The go-to-goal vector is towards up-right direction while the obstacle avoidance vector is towards the down-left direction. In this situation, the robot switches to follow wall behavior and keeps doing so until the conflict is resolved. After that, the robot switches to go-to-goal behavior.

 

Check my Github page for complete implementation.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google+ photo

You are commenting using your Google+ account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s