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_z = z_d(t) - z_fb(t);
% 反馈控制信号
control_signal_x = Kp(1) * error_x;
control_signal_y = Kp(2) * error_y;
control_signal_z = Kp(3) * error_z;
% 更新位置
x_fb(t+1) = x_fb(t) + control_signal_x * dt;
y_fb(t+1) = y_fb(t) + control_signal_y * dt;
z_fb(t+1) = z_fb(t) + control_signal_z * dt;
end
% 前馈+反馈控制仅仅比例控制
for t = 1:length(time)-1
% 当前误差
error_x = x_d(t) - x_fffb(t);
error_y = y_d(t) - y_fffb(t);
error_z = z_d(t) - z_fffb(t);
% 前馈控制信号:期望轨迹求导数way1的方式进行。
feedforward_x = -sin(time(t));
feedforward_y = cos(time(t));
feedforward_z = 0.5;
% 反馈控制信号
feedback_x = Kp(1) * error_x;
feedback_y = Kp(2) * error_y;
feedback_z = Kp(3) * error_z;
% 综合控制信号
control_signal_x = feedforward_x + feedback_x;
control_signal_y = feedforward_y + feedback_y;
control_signal_z = feedforward_z + feedback_z;
% 更新位置
x_fffb(t+1) = x_fffb(t) + control_signal_x * dt;
y_fffb(t+1) = y_fffb(t) + control_signal_y * dt;
z_fffb(t+1) = z_fffb(t) + control_signal_z * dt;
end
% PID控制
for t = 1:length(time)-1
% 当前误差
error_x = x_d(t) - x_pid(t);
error_y = y_d(t) - y_pid(t);
error_z = z_d(t) - z_pid(t);
% 积分误差
integrated_error(1) = integrated_error(1) + error_x * dt;
integrated_error(2) = integrated_error(2) + error_y * dt;
integrated_error(3) = integrated_error(3) + error_z * dt;
% 导数误差
derivative_error_x = (error_x - previous_error(1)) / dt;
derivative_error_y = (error_y - previous_error(2)) / dt;
derivative_error_z = (error_z - previous_error(3)) / dt;
% PID控制信号
control_signal_x = Kp(1) * error_x + Ki(1) * integrated_error(1) + Kd(1) * derivative_error_x;
control_signal_y = Kp(2) * error_y + Ki(2) * integrated_error(2) + Kd(2) * derivative_error_y;
control_signal_z = Kp(3) * error_z + Ki(3) * integrated_error(3) + Kd(3) * derivative_error_z;
% 更新位置
x_pid(t+1) = x_pid(t) + control_signal_x * dt;
y_pid(t+1) = y_pid(t) + control_signal_y * dt;
z_pid(t+1) = z_pid(t) + control_signal_z * dt;
% 更新前一个误差
previous_error = [error_x, error_y, error_z];
end
% 自适应PID控制
for t = 1:length(time)-1
% 当前误差
error_x = x_d(t) - x_apid(t);
error_y = y_d(t) - y_apid(t);
error_z = z_d(t) - z_apid(t);
% 自适应PID参数
adaptive_Kp = Kp + alpha .* abs([error_x, error_y, error_z]);
% 积分误差
integrated_error(1) = integrated_error(1) + error_x * dt;
integrated_error(2) = integrated_error(2) + error_y * dt;
integrated_error(3) = integrated_error(3) + error_z * dt;
% 导数误差
derivative_error_x = (error_x - previous_error(1)) / dt;
derivative_error_y = (error_y - previous_error(2)) / dt;
derivative_error_z = (error_z - previous_error(3)) / dt;
% 自适应PID控制信号
control_signal_x = adaptive_Kp(1) * error_x + Ki(1) * integrated_error(1) + Kd(1) * derivative_error_x;
control_signal_y = adaptive_Kp(2) * error_y + Ki(2) * integrated_error(2) + Kd(2) * derivative_error_y;
control_signal_z = adaptive_Kp(3) * error_z + Ki(3) * integrated_error(3) + Kd(3) * derivative_error_z;
% 更新位置
x_apid(t+1) = x_apid(t) + control_signal_x * dt;
y_apid(t+1) = y_apid(t) + control_signal_y * dt;
z_apid(t+1) = z_apid(t) + control_signal_z * dt;
% 更新前一个误差
previous_error = [error_x, error_y, error_z];
end
%通过前馈控制提供初始控制信号,减少误差的产生;通过反馈控制修正剩余误差
% 自适应PID+前馈控制
% Ki=diag(zeros(3))';Kd=zeros(1,3);
% %此部分注释掉之后,只剩自适应P控制+前馈,效果要好于自适应PID+前馈和固定P反馈控制+前馈控制
%这是因为由于前馈控制的引入已经将误差引入到较小情况,而较小的误差时,ID控制反而会对误差处理有较大影响
% 应在不同的使积分和微分部分在特定情况下起作用,比如误差较大或较小时分别调整不同的权重。
% 前馈控制提供了基于期望轨迹的初始控制信号,若前馈控制信号已经较好地接近实际需求,比例控制的适应性调整就足够了。
% 而PID控制中的积分和微分部分可能会对前馈信号进行不必要的修正,反而影响效果。
for t = 1:length(time)-1
% 当前误差
error_x = x_d(t) - x_apid_ff(t);
error_y = y_d(t) - y_apid_ff(t);
error_z = z_d(t) - z_apid_ff(t);
% 自适应PID参数
adaptive_Kp = Kp + alpha .* abs([error_x, error_y, error_z]);
% 积分误差
integrated_error(1) = integrated_error(1) + error_x * dt;
integrated_error(2) = integrated_error(2) + error_y * dt;
integrated_error(3) = integrated_error(3) + error_z * dt;
% 导数误差
derivative_error_x = (error_x - previous_error(1)) / dt;
derivative_error_y = (error_y - previous_error(2)) / dt;
derivative_error_z = (error_z - previous_error(3)) / dt;
% 自适应PID控制信号
control_signal_x = adaptive_Kp(1) * error_x + Ki(1) * integrated_error(1) + Kd(1) * derivative_error_x;
control_signal_y = adaptive_Kp(2) * error_y + Ki(2) * integrated_error(2) + Kd(2) * derivative_error_y;
control_signal_z = adaptive_Kp(3) * error_z + Ki(3) * integrated_error(3) + Kd(3) * derivative_error_z;
% 前馈控制信号
feedforward_x = -sin(time(t));
feedforward_y = cos(time(t));
feedforward_z = 0.5;
% 综合控制信号
control_signal_x = feedforward_x + control_signal_x;
control_signal_y = feedforward_y + control_signal_y;
control_signal_z = feedforward_z + control_signal_z;
% 更新位置
x_apid_ff(t+1) = x_apid_ff(t) + control_signal_x * dt;
y_apid_ff(t+1) = y_apid_ff(t) + control_signal_y * dt;
z_apid_ff(t+1) = z_apid_ff(t) + control_signal_z * dt;
% 更新前一个误差
previous_error = [error_x, error_y, error_z];
end
% 自适应PID+前馈控制---上一部分的修改,用于根据误差动态调整对微分和积分控制,稍微比前面的自适应PID好,效果是误差在前馈+反馈P控制附近震荡
% previous_error = [0; 0; 0];
% integrated_error = [0; 0; 0];
%
% for t = 1:length(time)-1
% % 当前误差
% error_x = x_d(t) - x_apid_ff(t);
% error_y = y_d(t) - y_apid_ff(t);
% error_z = z_d(t) - z_apid_ff(t);
%
% % 自适应PID参数
% adaptive_Kp = Kp + alpha .* abs([error_x, error_y, error_z]);
%
% % 积分误差,条件限制积分项。使积分和微分部分在特定情况下起作用,比如误差较大或较小时分别调整不同的权重。
% if abs(error_x) < 0.1
% integrated_error(1) = integrated_error(1) + error_x * dt;
% end
% if abs(error_y) < 0.1
% integrated_error(2) = integrated_error(2) + error_y * dt;
% end
% if abs(error_z) < 0.1
% integrated_error(3) = integrated_error(3) + error_z * dt;
% end
%
% % 导数误差,使用平滑处理
% derivative_error_x = (error_x - previous_error(1)) / dt;
% derivative_error_y = (error_y - previous_error(2)) / dt;
% derivative_error_z = (error_z - previous_error(3)) / dt;
% derivative_error_x = lowpass(derivative_error_x, 0.1, 1/dt); % 低通滤波
% derivative_error_y = lowpass(derivative_error_y, 0.1, 1/dt);
% derivative_error_z = lowpass(derivative_error_z, 0.1, 1/dt);
%
% % 自适应PID控制信号
% control_signal_x = adaptive_Kp(1) * error_x + Ki(1) * integrated_error(1) + Kd(1) * derivative_error_x;
% control_signal_y = adaptive_Kp(2) * error_y + Ki(2) * integrated_error(2) + Kd(2) * derivative_error_y;
% control_signal_z = adaptive_Kp(3) * error_z + Ki(3) * integrated_error(3) + Kd(3) * derivative_error_z;
%
% % 前馈控制信号
% feedforward_x = -sin(time(t));
% feedforward_y = cos(time(t));
% feedforward_z = 0.5;
%
% % 综合控制信号
% control_signal_x = feedforward_x + control_signal_x;
% control_signal_y = feedforward_y + control_signal_y;
% control_signal_z = feedforward_z + control_signal_z;
%
% % 更新位置
% x_apid_ff(t+1) = x_apid_ff(t) + control_signal_x * dt;
% y_apid_ff(t+1) = y_apid_ff(t) + control_signal_y * dt;
% z_apid_ff(t+1) = z_apid_ff(t) + control_signal_z * dt;
%
% % 更新前一个误差
% previous_error = [error_x, error_y, error_z];
% end
% 绘制轨迹比较
figure;
plot3(x_d, y_d, z_d, 'r', 'LineWidth', 2); hold on;
plot3(x_ff, y_ff, z_ff, 'b--', 'LineWidth', 2);
plot3(x_fb, y_fb, z_fb, 'g--', 'LineWidth', 2);
plot3(x_fffb, y_fffb, z_fffb, 'k--', 'LineWidth', 2);
plot3(x_pid, y_pid, z_pid, 'c--', 'LineWidth', 2);
plot3(x_apid, y_apid, z_apid, 'm--', 'LineWidth', 2);
plot3(x_apid_ff, y_apid_ff,z_apid_ff, 'k:', 'LineWidth', 2); % 自适应PID+前馈
xlabel('X');
ylabel('Y');
zlabel('Z');
legend('Desired', 'Feedforward', 'Feedback', 'Feedforward+Feedback', 'PID', 'Adaptive PID', 'Adaptive PID+Feedforward');
title('3D Trajectory Tracking Comparison');
grid on;
% 计算误差
error_ff = sqrt((x_d - x_ff).^2 + (y_d - y_ff).^2 + (z_d - z_ff).^2);
error_fb = sqrt((x_d - x_fb).^2 + (y_d - y_fb).^2 + (z_d - z_fb).^2);
error_fffb = sqrt((x_d - x_fffb).^2 + (y_d - y_fffb).^2 + (z_d - z_fffb).^2);
error_pid = sqrt((x_d - x_pid).^2 + (y_d - y_pid).^2 + (z_d - z_pid).^2);
error_apid = sqrt((x_d - x_apid).^2 + (y_d - y_apid).^2 + (z_d - z_apid).^2);
error_apid_ff = sqrt((x_d - x_apid_ff).^2 + (y_d - y_apid_ff).^2 + (z_d - z_apid_ff).^2);
% 绘制误差比较
figure;
plot(time, error_ff, 'b', 'LineWidth', 2); hold on;
plot(time, error_fb, 'g', 'LineWidth', 2);
plot(time, error_fffb, 'k', 'LineWidth', 2);
plot(time, error_pid, 'c', 'LineWidth', 2);
plot(time, error_apid, 'm', 'LineWidth', 2);
plot(time, error_apid_ff, 'k:', 'LineWidth', 2); % 自适应PID+前馈
xlabel('Time');
ylabel('Error');
legend('Feedforward', 'Feedback', 'Feedforward+Feedback', 'PID', 'Adaptive PID', 'Adaptive PID+Feedforward');
title('Error Comparison');
grid on;
3、结果【前馈的引入效果好很多】
4、其他参考学习链接
什么是前馈控制?_哔哩哔哩_bilibili
只用PID?加上前馈解决95%的问题!_哔哩哔哩_bilibili
PID控制器开发笔记之九:基于前馈补偿的PID控制器的实现_前馈补偿 c语言-CSDN博客
PID算法-从单级PID到单神经元自适应PID控制器 - 任囧 - 博客园 (cnblogs.com)
线性二次型调节器(LQR)原理详解-CSDN博客
前馈控制与MPC - Henry的博客 | Henry Blog (ldylab.cc)