Matlab 的非线性 mpc 导致 u0 控制输入始终

如何解决Matlab 的非线性 mpc 导致 u0 控制输入始终

我正在使用 MATLAB 的非线性 MPC 工具箱并尝试实现一些简单的控制器。但是，给定初始控制信号 uk，nlmpcmove 生成​​的任何控制信号都与 uk 相同（saveStates 填充了零时间的任何 uk）。

相关代码部分；

%% Create MPC object
numStates = 12;
numOutputs = 12;
numControl = 2;

nlobj = nlmpc(numStates,numOutputs,numControl);
nlobj.Ts = constants.Ts;
Tswing = 0.1;
nlobj.PredictionHorizon = 5;
nlobj.ControlHorizon = 4;

nlobj.Model.StateFcn = @legStateFcn;
nlobj.Model.IsContinuousTime = false;

% Output function is exact state
nlobj.Model.OutputFcn = @(x,u) x;
nlobj.Jacobian.OutputFcn = @(x,u) eye(numStates);

% Weights
nlobj.Optimization.CustomCostFcn = @costFcn;
nlobj.Optimization.ReplaceStandardcost = true;

nlobj.Optimization.CustomineqConFcn = @ineqFcn;

% Kalman filter
EKF = extendedKalmanFilter(@legStateFcn,@legMeasurementFcn);
EKF.State = x0;

%% Run MPC
u0 = zeros(numControl,1);
uk = u0;

yinit = x0;
yfinal = x0 + 1;
yref = [yinit yfinal]';

validateFcns(nlobj,yinit,u0) % THIS SAYS IT IS ALL OK

saveState = zeros(length(yinit),length(t));
saveControl = zeros(length(uk),length(t));
y = yinit;
for i = 1:length(t)
    disp(i/length(t)*100)
    
    xk = correct(EKF,y);
    saveState(:,i) = xk;
    
    uk = nlmpcmove(nlobj,xk,uk,yref(2,:));
    saveControl(:,i) = uk;

    % Predict state for next iteration
    predict(EKF,uk);
    % Implement optimal control move
    x = legStateFcn(xk,uk);
    y = x;    
end

由于 validateFcns(nlobj,u0) 返回所有 OK，所以我没有理由向您展示所有函数句柄，但只是为了更好的衡量；

LegStateFcn.m; function xk1 = legStateFcn(xk,uk)

costFcn.m; function J = costFcn(X,U,e,data)

ineqFcn.m; function ineq = ineqFcn(X,data)

legMeasurementFcn.m; function yk = legMeasurementFcn(xk)

MATLAB 不会抱怨我在这里所做的任何事情，但是，鉴于在 u0 之前给出的任何 for-loop，来自 nlmpcmove 的任何结果控制信号都是相同的 u0 .我通过在 uk 之前和之后重命名 nlmpcmove 进行调试，删除 nlobj.Jacobian.OutputFcn，模拟结果状态（不遵守不等式约束）并看到它确实遵循正如我所期望的那样，系统的动态性。

我不知道我做错了什么，我担心这是一个如此明显的小错误，我一开始就不应该犯。