Simulink model for Turtlebot movement along a trajectory avoiding objects
I am working on creating a model in Simulink that allows me to control a Turtlebot waffle pi so that it moves along a 6-point path. Along the path there will be obstacles in the form of red and green spheres. The objective is for the robot to reach all the points of the path avoiding the obstacles, if the sphere is red, avoid to the right and if it is green, avoid to the left.
I have already created the following Simulink model where the most important thing is in the control function since it describes the behavior of the robot, however the error that keeps appearing is the following: Error: Block ”Project_3/MATLAB Function” does not fully set the dimensions of output ‘next_index’. Which I do not find sense since the variable next_index is defined from the beginning. I attach the images of the model that I am creating.
function [area, x_obst, y_obst, target_x, target_y, obstacle, current_index, target_theta,last_point] = WaypointSelector(a, cent, waypoints, current_index)
%Extract obstacle’s area and position
[~,i] = max(a);
area = max(a);
if ~isempty(i)
obstacle = cent(i,:);
else
obstacle = [0,0];
end
% Cordanates of the obstacles
x_obst = obstacle(1);
y_obst = obstacle(2);
% Extract target waypoint
target_x = waypoints(current_index, 1);
target_y = waypoints(current_index, 2);
last_point = size(waypoints, 1);
target_theta = waypoints(current_index, 3);
target_theta = atan2(sin(target_theta), cos(target_theta));
end
CONTROL FUNCTION:
function [v, omega, next_index] = ComputeControl(target_x, target_y, obstacle, current_index, target_theta, last_point, current_x, current_y, x_ang, y_ang, z_ang, w_ang, ranges)
% Constants
max_velocity = 0.2; % max linear velocity
max_yaw_rate = 0.4; % max angular velocity
max_distance = 0.05; % max distance to consider waypoint reached
l = 640; % size of the camera’s image
h = 480;
% Initialize output
next_index = int32(current_index); % Initialize as integer (or the appropriate type)
% Coordinates of the obstacles
x_obst = obstacle(1);
y_obst = obstacle(2);
% Position and direction of the robot
current_angle = quat2eul([x_ang y_ang z_ang w_ang]);
yaw_angle = current_angle(3);
if yaw_angle > pi
current_theta = yaw_angle – 2*pi;
else
current_theta = yaw_angle;
end
% Compute distance and angle to the target waypoint
dx = target_x – current_x;
dy = target_y – current_y;
distance = sqrt(dx^2 + dy^2); % Distance to the waypoints
dist_obs = min(ranges); % Distance to the closest obstacle (Radar)
angle_to_target = atan2(dy, dx); % Angle needed to reach the waypoints
% Adjust the angle to be in the range [-pi, pi]
if dx < 0
if dy > 0
angle_to_target = angle_to_target + pi;
else
angle_to_target = angle_to_target – pi;
end
end
% Compute delta theta and the error in the direction
d_theta = target_theta – current_theta;
angle_error = current_theta – angle_to_target;
% Compute the control logic commands to achieve the waypoints and avoid the obstacles
if (x_obst – l/2) > 0
omega = max_yaw_rate;
v = max_velocity;
else
if current_index > last_point
v = 0;
omega = 0;
next_index = current_index;
else
if distance <= max_distance && abs(d_theta) <= 0.05
next_index = current_index + 1;
else
next_index = current_index; % Ensure next_index is assigned
end
if distance > max_distance
if abs(angle_error) > 0.01
if abs(angle_error) > 0.5
omega = max_yaw_rate;
elseif abs(angle_error) > 0.3 && abs(angle_error) <= 0.5
omega = 0.5 * max_yaw_rate;
else
omega = 0.1 * max_yaw_rate;
end
if current_theta > angle_to_target && current_index < 7
omega = -omega;
end
if distance >= 3 * max_distance
v = max_velocity;
else
v = max_velocity * (distance / (distance + 1));
end
else
omega = 0;
if distance >= 3 * max_distance
v = max_velocity;
else
v = max_velocity * (distance / (distance + 1));
end
end
else
if abs(d_theta) > 0.05
omega = max_yaw_rate * d_theta;
v = 0;
if current_theta > target_theta
omega = -omega;
end
else
omega = 0;
v = 0;
end
end
end
end
% Limit the velocities just in case
v = min(v, max_velocity);
omega = min(max(omega, -max_yaw_rate), max_yaw_rate);
endI am working on creating a model in Simulink that allows me to control a Turtlebot waffle pi so that it moves along a 6-point path. Along the path there will be obstacles in the form of red and green spheres. The objective is for the robot to reach all the points of the path avoiding the obstacles, if the sphere is red, avoid to the right and if it is green, avoid to the left.
I have already created the following Simulink model where the most important thing is in the control function since it describes the behavior of the robot, however the error that keeps appearing is the following: Error: Block ”Project_3/MATLAB Function” does not fully set the dimensions of output ‘next_index’. Which I do not find sense since the variable next_index is defined from the beginning. I attach the images of the model that I am creating.
function [area, x_obst, y_obst, target_x, target_y, obstacle, current_index, target_theta,last_point] = WaypointSelector(a, cent, waypoints, current_index)
%Extract obstacle’s area and position
[~,i] = max(a);
area = max(a);
if ~isempty(i)
obstacle = cent(i,:);
else
obstacle = [0,0];
end
% Cordanates of the obstacles
x_obst = obstacle(1);
y_obst = obstacle(2);
% Extract target waypoint
target_x = waypoints(current_index, 1);
target_y = waypoints(current_index, 2);
last_point = size(waypoints, 1);
target_theta = waypoints(current_index, 3);
target_theta = atan2(sin(target_theta), cos(target_theta));
end
CONTROL FUNCTION:
function [v, omega, next_index] = ComputeControl(target_x, target_y, obstacle, current_index, target_theta, last_point, current_x, current_y, x_ang, y_ang, z_ang, w_ang, ranges)
% Constants
max_velocity = 0.2; % max linear velocity
max_yaw_rate = 0.4; % max angular velocity
max_distance = 0.05; % max distance to consider waypoint reached
l = 640; % size of the camera’s image
h = 480;
% Initialize output
next_index = int32(current_index); % Initialize as integer (or the appropriate type)
% Coordinates of the obstacles
x_obst = obstacle(1);
y_obst = obstacle(2);
% Position and direction of the robot
current_angle = quat2eul([x_ang y_ang z_ang w_ang]);
yaw_angle = current_angle(3);
if yaw_angle > pi
current_theta = yaw_angle – 2*pi;
else
current_theta = yaw_angle;
end
% Compute distance and angle to the target waypoint
dx = target_x – current_x;
dy = target_y – current_y;
distance = sqrt(dx^2 + dy^2); % Distance to the waypoints
dist_obs = min(ranges); % Distance to the closest obstacle (Radar)
angle_to_target = atan2(dy, dx); % Angle needed to reach the waypoints
% Adjust the angle to be in the range [-pi, pi]
if dx < 0
if dy > 0
angle_to_target = angle_to_target + pi;
else
angle_to_target = angle_to_target – pi;
end
end
% Compute delta theta and the error in the direction
d_theta = target_theta – current_theta;
angle_error = current_theta – angle_to_target;
% Compute the control logic commands to achieve the waypoints and avoid the obstacles
if (x_obst – l/2) > 0
omega = max_yaw_rate;
v = max_velocity;
else
if current_index > last_point
v = 0;
omega = 0;
next_index = current_index;
else
if distance <= max_distance && abs(d_theta) <= 0.05
next_index = current_index + 1;
else
next_index = current_index; % Ensure next_index is assigned
end
if distance > max_distance
if abs(angle_error) > 0.01
if abs(angle_error) > 0.5
omega = max_yaw_rate;
elseif abs(angle_error) > 0.3 && abs(angle_error) <= 0.5
omega = 0.5 * max_yaw_rate;
else
omega = 0.1 * max_yaw_rate;
end
if current_theta > angle_to_target && current_index < 7
omega = -omega;
end
if distance >= 3 * max_distance
v = max_velocity;
else
v = max_velocity * (distance / (distance + 1));
end
else
omega = 0;
if distance >= 3 * max_distance
v = max_velocity;
else
v = max_velocity * (distance / (distance + 1));
end
end
else
if abs(d_theta) > 0.05
omega = max_yaw_rate * d_theta;
v = 0;
if current_theta > target_theta
omega = -omega;
end
else
omega = 0;
v = 0;
end
end
end
end
% Limit the velocities just in case
v = min(v, max_velocity);
omega = min(max(omega, -max_yaw_rate), max_yaw_rate);
end I am working on creating a model in Simulink that allows me to control a Turtlebot waffle pi so that it moves along a 6-point path. Along the path there will be obstacles in the form of red and green spheres. The objective is for the robot to reach all the points of the path avoiding the obstacles, if the sphere is red, avoid to the right and if it is green, avoid to the left.
I have already created the following Simulink model where the most important thing is in the control function since it describes the behavior of the robot, however the error that keeps appearing is the following: Error: Block ”Project_3/MATLAB Function” does not fully set the dimensions of output ‘next_index’. Which I do not find sense since the variable next_index is defined from the beginning. I attach the images of the model that I am creating.
function [area, x_obst, y_obst, target_x, target_y, obstacle, current_index, target_theta,last_point] = WaypointSelector(a, cent, waypoints, current_index)
%Extract obstacle’s area and position
[~,i] = max(a);
area = max(a);
if ~isempty(i)
obstacle = cent(i,:);
else
obstacle = [0,0];
end
% Cordanates of the obstacles
x_obst = obstacle(1);
y_obst = obstacle(2);
% Extract target waypoint
target_x = waypoints(current_index, 1);
target_y = waypoints(current_index, 2);
last_point = size(waypoints, 1);
target_theta = waypoints(current_index, 3);
target_theta = atan2(sin(target_theta), cos(target_theta));
end
CONTROL FUNCTION:
function [v, omega, next_index] = ComputeControl(target_x, target_y, obstacle, current_index, target_theta, last_point, current_x, current_y, x_ang, y_ang, z_ang, w_ang, ranges)
% Constants
max_velocity = 0.2; % max linear velocity
max_yaw_rate = 0.4; % max angular velocity
max_distance = 0.05; % max distance to consider waypoint reached
l = 640; % size of the camera’s image
h = 480;
% Initialize output
next_index = int32(current_index); % Initialize as integer (or the appropriate type)
% Coordinates of the obstacles
x_obst = obstacle(1);
y_obst = obstacle(2);
% Position and direction of the robot
current_angle = quat2eul([x_ang y_ang z_ang w_ang]);
yaw_angle = current_angle(3);
if yaw_angle > pi
current_theta = yaw_angle – 2*pi;
else
current_theta = yaw_angle;
end
% Compute distance and angle to the target waypoint
dx = target_x – current_x;
dy = target_y – current_y;
distance = sqrt(dx^2 + dy^2); % Distance to the waypoints
dist_obs = min(ranges); % Distance to the closest obstacle (Radar)
angle_to_target = atan2(dy, dx); % Angle needed to reach the waypoints
% Adjust the angle to be in the range [-pi, pi]
if dx < 0
if dy > 0
angle_to_target = angle_to_target + pi;
else
angle_to_target = angle_to_target – pi;
end
end
% Compute delta theta and the error in the direction
d_theta = target_theta – current_theta;
angle_error = current_theta – angle_to_target;
% Compute the control logic commands to achieve the waypoints and avoid the obstacles
if (x_obst – l/2) > 0
omega = max_yaw_rate;
v = max_velocity;
else
if current_index > last_point
v = 0;
omega = 0;
next_index = current_index;
else
if distance <= max_distance && abs(d_theta) <= 0.05
next_index = current_index + 1;
else
next_index = current_index; % Ensure next_index is assigned
end
if distance > max_distance
if abs(angle_error) > 0.01
if abs(angle_error) > 0.5
omega = max_yaw_rate;
elseif abs(angle_error) > 0.3 && abs(angle_error) <= 0.5
omega = 0.5 * max_yaw_rate;
else
omega = 0.1 * max_yaw_rate;
end
if current_theta > angle_to_target && current_index < 7
omega = -omega;
end
if distance >= 3 * max_distance
v = max_velocity;
else
v = max_velocity * (distance / (distance + 1));
end
else
omega = 0;
if distance >= 3 * max_distance
v = max_velocity;
else
v = max_velocity * (distance / (distance + 1));
end
end
else
if abs(d_theta) > 0.05
omega = max_yaw_rate * d_theta;
v = 0;
if current_theta > target_theta
omega = -omega;
end
else
omega = 0;
v = 0;
end
end
end
end
% Limit the velocities just in case
v = min(v, max_velocity);
omega = min(max(omega, -max_yaw_rate), max_yaw_rate);
end simulink, turtlebot, matlab, control, gazebo, ros, image processing, motion, error MATLAB Answers — New Questions
