TEB路径规划matlab代码

以下是一个简单的 TEB (Time-Elastic Band) 路径规划的 Matlab 代码示例：

% 定义障碍物
obs = [1.5, 1.5; 1.5, 3.5; 3.5, 3.5; 3.5, 1.5];
% 定义起点和终点
start = [0, 0];
goal = [4, 4];
% 定义TEB参数
robot_radius = 0.5;
max_vel = 1;
max_acc = 1;
dt = 0.1;
% 调用TEB路径规划函数
path = teb_planner(start, goal, obs, robot_radius, max_vel, max_acc, dt);
% 绘制路径和障碍物
plot(obs(:,1), obs(:,2), 'k', 'LineWidth', 2);
hold on;
plot(path(:,1), path(:,2), 'b', 'LineWidth', 2);
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'LineWidth', 2);
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
axis equal;

其中 `teb_planner` 是一个自定义函数，实现TEB路径规划算法。注意，这只是一个简单的示例，实际应用中需要更复杂的障碍物和环境模型，以及更多的参数调整和优化。