matlab 轨迹跟踪程序

以下是一个简单的 Matlab 轨迹跟踪程序，可以帮助你开始学习轨迹跟踪：

clear all;
clc;

% 设定时间步长
dt = 0.01;

% 设定目标点坐标
x_goal = 5;
y_goal = 3;

% 设定小车起始位置
x_init = 0;
y_init = 0;
theta_init = 0;

% 设定控制器增益
Kp = 1;
Kd = 0.5;

% 设定小车初始速度和角速度
v = 0;
w = 0;

% 设定仿真时间
t_final = 10;
t = 0:dt:t_final;

% 初始化小车位置和速度
x(1) = x_init;
y(1) = y_init;
theta(1) = theta_init;
vx(1) = v;
vy(1) = 0;
omega(1) = w;

% 开始仿真
for i = 2:length(t)
    % 计算小车到目标点的距离和角度
    dx = x_goal - x(i-1);
    dy = y_goal - y(i-1);
    d = sqrt(dx^2 + dy^2);
    theta_d = atan2(dy, dx);
    
    % 计算控制信号
    e = theta_d - theta(i-1);
    if e > pi
        e = e - 2*pi;
    elseif e < -pi
        e = e + 2*pi;
    end
    u = Kp*e + Kd*(e - (theta(i-1) - theta(i-2))/dt);
    
    % 计算小车的加速度和角加速度
    a = u;
    alpha = (a - omega(i-1)*vy(i-1))/vx(i-1);
    
    % 更新小车速度和位置
    vx(i) = vx(i-1) + a*dt;
    vy(i) = vy(i-1) + alpha*vx(i-1)*dt;
    v = sqrt(vx(i)^2 + vy(i)^2);
    omega(i) = alpha*v;
    
    x(i) = x(i-1) + vx(i)*cos(theta(i-1))*dt;
    y(i) = y(i-1) + vy(i)*sin(theta(i-1))*dt;
    theta(i) = theta(i-1) + omega(i)*dt;
end

% 绘制小车轨迹和目标点
plot(x, y);
hold on;
plot(x_goal, y_goal, 'ro');
xlabel('x');
ylabel('y');
title('小车轨迹跟踪');

这个程序使用基本的 PID 控制器来跟踪给定的目标点。你可以根据自己的需要更改控制器增益和目标点坐标，以及添加其他功能来改进程序。