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).
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.
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 ( and ) is a function of ticks per revolution of the wheels (). and can be used to calculate the new state of the system as shown below.
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 .
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 &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 &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 . 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 from IR sensor can be written as vector from sensor’s frame of reference. This point can be transformed to robot’s frame of reference by multiplying it to transformation matrix . Here is the pose of the sensor in robot’s frame of reference. The resulting vector is transformed to the world’s reference frame by multiplying it to another transformation matrix . Here is the pose of the robot in world’s frame of reference. The resulting vector is the position of the point from world’s frame of reference.
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 . This vector along with robot pose information can be used to find the optimal heading angle () to drive the robot away from the obstacle. This angle is then compared with current robot angle () to get error signal for the PID controller.
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 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.
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 ( and ) to form a new direction vector (). Using this vector we would find out the error in current robot angle and implement a PID controller to reduce the error.
% 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;lt;span &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;gt;&amp;amp;amp;amp;amp;lt;/span&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 direction towards the goal. Whenever the robot detects an obstacle with its IR sensors it would switch to the blended controller to move in direction. Further, if the distance from the obstacle is too low (unsafe) it would switch to the obstacle avoidance controller to move in 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.
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 is the sensor distance vector with smallest IR distance readings and is its neighbor sensor distance vector detecting an obstacle the vector tangent to the obstacle boundary () can be calculated as difference of and vectors. is the normalized vector along the obstacle boundary. represents the vector perpendicular to the obstacle boundary from the robot position. is the vector pointing towards the obstacle when the distance to the obstacle is more than , is near zero when the robot is away from the obstacle, and points away from the obstacle when distance to the obstacle is less than . The net follow wall vector () is calculated as linear combination of and .
% 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')... && ~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].
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.