% Segway式机器人平台轨迹跟踪基于MPC

% 参数设定
Ts = 0.1; % 采样时间
N = 10; % 控制时域长度
Q = 1; % 状态权重
R = 0.1; % 控制权重

% 状态空间模型
A = [1 Ts; 0 1];
B = [0; Ts];
C = [1 0];

% 初始状态
x0 = [0; 0];

% 参考轨迹
t = 0:Ts:10;
desired_angle = sin(t);

% MPC
controller = mpc(A, B, C, [], N);
controller.Model.Plant = ss(A, B, C, 0, Ts);
controller.W.Output = Q;
controller.W.ManipulatedVariable = R;
controller.W.ManipulatedVariableRate = 0.1;

simulated_angle = zeros(size(t));
x = x0;
for k = 1:length(t)
r = desired_angle(k);
u = mpcmove(controller, x, r);
x = Ax + Bu;
simulated_angle(k) = C*x;
end

% 绘图
figure;
plot(t, desired_angle, ‘b–’, t, simulated_angle, ‘r-’);
xlabel(‘时间’);
ylabel(‘角度’);
legend(‘参考角度’, ‘模拟角度’);
title(‘Segway式机器人平台轨迹跟踪’);

% 输出角度
disp(‘模拟角度:’);
disp(simulated_angle);