Simulink model for Turtlebot movement along a trajectory avoiding objects
47 次查看(过去 30 天)
显示 更早的评论
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.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1757294/image.png)
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1757299/image.png)
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
2 个评论
回答(2 个)
Aravind
2024-8-23,8:48
Hi,
In your Simulink model, I noticed that you have two “MATLAB Function” blocks arranged in series. When “MATLAB Function“ blocks are placed in series, it can often be challenging for the solver to determine the appropriate signal dimension sizes if the output sizes are set to “inherited”.
To address this issue, you can manually specify the signal sizes by following these steps:
- Click into the “MATLAB Function” block. This will open a script within MATLAB.
- Under the 'FUNCTION' tab, there should be a button labeled 'Edit Data'. Click on it.
- Select the variable that is used as an output from the “MATLAB Function” block.
- Change the 'size' from -1, which is “inherited”, to the dimension that the output should be. In your current example, the 'size' of “next_index” must be set to 1.
- Specify the dimensions for the other outputs if needed as well.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1758534/image.png)
I have attached a screenshot of the above procedure for your reference. Hope this helps!
0 个评论
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!