轨迹跟踪PID相关及前馈引入结果对比

1、前言：

        记录下给定一段轨迹如何实现跟踪控制，主要使用几个经典的方法进行对比。一些讨论的部分在代码中的注释部分。

        前馈控制可以在没有扰动和建模误差的理想条件下很好地跟踪期望输出，但在实际系统中，由于存在建模误差、外部扰动和初始条件误差，前馈控制无法完全消除系统误差。通常，前馈控制与反馈控制结合使用，通过反馈控制来补偿和校正误差，从而实现更精确和鲁棒的控制系统。

2、代码

% 参数设置
dt = 0.01;
T = 10;
time = 0:dt:T;

% 期望轨迹
x_d = cos(time); % x轨迹
y_d = sin(time); % y轨迹
z_d = 0.5 * time; % z轨迹

% 初始化位置
x_ff = zeros(size(time));
y_ff = zeros(size(time));
z_ff = zeros(size(time));

x_fb = zeros(size(time));
y_fb = zeros(size(time));
z_fb = zeros(size(time));

x_fffb = zeros(size(time));
y_fffb = zeros(size(time));
z_fffb = zeros(size(time));

x_pid = zeros(size(time));
y_pid = zeros(size(time));
z_pid = zeros(size(time));

x_apid = zeros(size(time));
y_apid = zeros(size(time));
z_apid = zeros(size(time));

x_apid_ff = zeros(size(time));
y_apid_ff = zeros(size(time));
z_apid_ff = zeros(size(time));
% x_apid_ff(1) = 1;

% PID参数
Kp = [20, 20, 20];
Ki = [0.1, 0.1, 0.1];
Kd = [0.01, 0.01, 0.01];

% 自适应PID参数
alpha = [20, 20, 20]; % 更新系数，控制参数的调整速度

% 初始点位置,说明初始误差的情况
x_ff(1)=.5;
x_fb(1) = .5;
x_fffb(1) = .5;
x_pid(1) = .5;
x_apid(1) = .5;
x_apid_ff(1)=.5;

% 初始化PID变量
integrated_error = [0, 0, 0];
previous_error = [0, 0, 0];


% 前馈控制
x_velocity_d = gradient(x_d, dt);%输出 FX 对应于 ∂F/∂x，即 x（水平）方向上的差分--近似导数--离散数据的梯度,way2---和diff(x_d) / dt同.但有较小误差
y_velocity_d = gradient(y_d, dt);
z_velocity_d = gradient(z_d, dt);
%{
% 数据： 说明gradient与diff使用之间的差别。diff由于是前向差分所以少一个数据。
x = 0:0.1:10;
y = sin(x);

% 使用 gradient 计算梯度
dydx_gradient = gradient(y, 0.1);

% 使用 diff 计算差分
dydx_diff = diff(y) / 0.1;
x_diff = x(1:end-1);

% 绘图
figure;
plot(x, y, 'r', 'LineWidth', 2); hold on;
plot(x, dydx_gradient, 'b--', 'LineWidth', 2);
plot(x_diff, dydx_diff, 'g-.', 'LineWidth', 2);
xlabel('X');
ylabel('Y and dY/dX');
legend('y = sin(x)', 'Gradient dy/dx', 'Diff dy/dx');
title('Comparison of Gradient and Diff');
grid on;

%}

for t = 1:length(time)-1
    % 前馈控制信号: 期望轨迹求导数way1；前馈输入为位置轨迹的差分或梯度
    % control_signal_x = -sin(time(t));
    % control_signal_y = cos(time(t));
    % control_signal_z = 0.5;
    % way2
% 获取期望速度（即前馈控制输入）way2
control_signal_x = x_velocity_d(t);
control_signal_y = y_velocity_d(t); 
control_signal_z =z_velocity_d(t);

    % 更新位置
    x_ff(t+1) = x_ff(t) + control_signal_x * dt;
    y_ff(t+1) = y_ff(t) + control_signal_y * dt;
    z_ff(t+1) = z_ff(t) + control_signal_z * dt;
end

% 反馈控制
for t = 1:length(time)-1
    % 当前误差
    error_x = x_d(t) - x_fb(t);
    error_y = y_d(t) - y_fb(t);
    error_