飞行机器人

这个例子中的飞行机器人有四个推进器在二维空间中移动。该模型有六个状态:

有关飞行机器人的更多信息，请参见[1]。本文模型采用两个推力，推力范围为-1 ~ 1。然而，这个例子假设机器人有四个物理推力，范围从0到1，以实现相同的控制自由度。

轨迹规划

机器人最初停留在(-10、-10)方向角为π/ 2弧度(朝北)。本例中的飞行机动是移动机器人并将其停在最终位置(0,0)以一个角度0(朝东的)弧度12秒。目标是找到最优路径，使推进器在机动过程中消耗的燃料总量最小化。

非线性MPC是解决轨迹规划问题的理想工具，因为它解决了给定当前装置状态的开环约束非线性优化问题。利用非线性动态模型，MPC可以做出更准确的决策。

在此例中，目标预测时间为12秒。因此，指定采样时间为0.4秒和预测视界30.步骤。创建多级非线性MPC对象6州和4输入。默认情况下，所有输入都是操作变量(mv)。

Ts = 0.4;P = 30;Nx = 6;Nu = 4;nlobj = nlmpcMultistage(p,nx,nu);nlobj。Ts = Ts;

对于路径规划问题，通常允许MPC在每个预测步骤有自由移动，这为优化问题提供了最大数量的决策变量。由于规划通常以比反馈控制器慢得多的采样率运行，由较大优化问题引入的额外计算负荷是可以接受的。

使用函数名指定预测模型状态函数。也可以使用函数句柄指定函数。关于状态功能的详细信息，请打开FlyingRobotStateFcn.m．有关指定预测模型的详细信息，请参见为非线性MPC指定预测模型．

nlobj.Model.StateFcn =“FlyingRobotStateFcn”；

使用函数句柄指定状态函数的雅可比矩阵。最好的做法是为预测模型提供一个解析雅可比矩阵。这样做可以显著提高模拟效率。有关雅可比函数的详细信息，请打开FlyingRobotStateJacobianFcn.m．

nlobj.Model.StateJacFcn = @FlyingRobotStateJacobianFcn;

轨迹规划问题通常涉及一个非线性代价函数，它可以用来寻找最短的距离，最大的利润，或者像在这种情况下，最小的燃料消耗。由于推力值是燃料消耗的直接指标，因此计算燃料成本为从阶段1到阶段p的每个预测步骤的推力值之和。使用命名函数指定此成本函数。有关指定成本函数的更多信息，请参见指定非线性MPC的代价函数．

为ct = 1:p nlobj.分期(ct)。CostFcn =“FlyingRobotCostFcn”；结束

机动的目标是将机器人停在(0,0)以一个角度0弧度在12秒。将这个目标指定为终端状态约束，其中最后一个预测步骤(阶段p+1)的每个位置和速度状态都应该为零。有关指定约束函数的详细信息，请参见为非线性MPC指定约束．

nlobj.Model.TerminalState = 0 (6,1);

最好的做法是为你的阶段成本和约束函数提供分析雅可比函数。然而，本例有意跳过它们，以便它们的雅可比矩阵由非线性MPC控制器使用内置的数值摄动方法计算。

每个推力之间都有一个工作范围0而且1，它被转换为mv的下界和上界。

为ct = 1:nu nlobj.MV(ct)。Min = 0;nlobj.MV (ct)。Max = 1;结束

为机器人指定初始条件。

X0 = [-10;-10;pi/2;0;0;0];机器人停在[-10，-10]，朝北U0 = 0 (nu,1);%推力为零

验证用户提供的模型、成本和约束函数及其雅可比矩阵是最佳实践。要做到这一点，请使用validateFcns命令。

validateFcns (nlobj x0,情况);

模型。StateFcn是OK的。模型。StateJacFcn是OK的。“CostFcn”以下阶段[1 23 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30]均OK。分析了用户提供的模型、成本和约束函数。

最优状态和MV轨迹可以通过调用nlmpcmove命令一次，给定当前状态x0最后MV情况．最优代价和轨迹作为信息输出参数。

[~，~，info] = nlmpcmove(nlobj,x0,u0);

画出最佳轨迹。最优成本是7.8。

FlyingRobotPlotPlanning(信息,Ts);

最佳油耗= 7.797825

第一个图显示了在机动过程中六个机器人状态的最佳轨迹。第二个图显示了四个逆冲对应的最佳MV剖面。第三个图显示了机器人的X-Y位置轨迹，从[-10 -10 pi/2]来[0 0 0]．

路径跟踪的反馈控制

在找到最优轨迹后，需要一个反馈控制器来使机器人沿着路径移动。理论上，可以将最优MV剖面直接应用于推进器实现前馈控制。然而，在实际应用中，需要一个反馈控制器来抑制干扰和补偿建模误差。

您可以使用不同的反馈控制技术进行跟踪。在本例中，您使用一个通用的非线性MPC控制器将机器人移动到最终位置。在这个路径跟踪问题中，跟踪所有六个状态的引用(输出的数量等于状态的数量)。

Ny = 6;Nlobj_tracking = nlmpc(nx,ny,nu);

在标准代价函数中，默认情况下对一个或多个ov应用零权重，因为mv比ov少。

使用相同的状态函数及其雅可比函数。

nlobj_tracking.Model。StateFcn = nlobj.Model.StateFcn;nlobj_tracking.Jacobian。StateFcn = nlobj. model . statjacfcn;

对于跟踪控制应用程序，通过指定更短的预测范围(不需要展望遥远的未来)和控制范围(例如，在前几个预测步骤分配自由移动)来减少计算工作量。

nlobj_tracking。Ts = Ts;nlobj_tracking。PredictionHorizon = 10;nlobj_tracking。ControlHorizon = 4;

非线性MPC的缺省代价函数是一个适用于参考跟踪和干扰抑制的标准二次代价函数。对于跟踪，跟踪误差具有更高的优先级(输出上的惩罚权重更大)，而不是控制努力(MV率上的惩罚权重更小)。

nlobj_tracking.Weights。manipulatedvariablerate = 0.2*ones(1,nu);nlobj_tracking.Weights。OutputVariables = 5*ones(1,nx);

为推进器输入设置相同的边界。

为ct = 1:nu nlobj_tracking.MV(ct)。Min = 0;nlobj_tracking.MV (ct)。Max = 1;结束

此外，要减少燃料消耗，显然u1而且u2在操作过程中任何时候都不能为正数。因此，实现相等约束使u (1) * (2)必须0对于所有的预测步骤。将类似的约束应用于u3而且的愉快．

nlobj_tracking.Optimization。CustomEqConFcn =.．.@ (X, U,数据)[U (1: end-1, 1)。* U (1: end-1, 2);U (1: end-1, 3) * U (1: end-1, 4)];

验证您的预测模型和自定义函数，以及它们的雅可比矩阵。

validateFcns (nlobj_tracking x0,情况);

模型。StateFcn是OK的。雅可比矩阵。StateFcn是OK的。没有指定输出函数。假设预测模型中“y = x”。优化。CustomEqConFcn是OK的。分析了用户提供的模型、成本和约束函数。

非线性状态估计

在这个例子中，只测量了三个位置状态(x、y和角度)。速度状态是无法测量的，必须估计出来。使用Control System Toolbox™中的扩展卡尔曼滤波器(EKF)进行非线性状态估计。

因为EKF需要离散时间模型，所以使用梯形规则从x(k)转换到x(k+1)，这需要解nx非线性代数方程。欲了解更多信息，请打开FlyingRobotStateFcnDiscreteTime.m．

DStateFcn = @(xk,uk,Ts) flyingrobotstatefcn离散时间(xk,uk,Ts);

测量可以帮助EKF纠正其状态估计。只测量前三个状态。

DMeasFcn = @(xk) xk(1:3);

创建EKF，并指出测量值几乎没有噪声。

EKF = extendedKalmanFilter(DStateFcn,DMeasFcn,x0);卡尔曼滤波器。测量噪声= 0.01;

跟踪控制的闭环仿真

模拟系统32具有正确初始条件的步骤。

步数= 32;xHistory = x0';uHistory = [];lastMV = 0 (nu,1);

参考信号是在规划阶段计算出的最优状态轨迹。当将这些轨迹传递给非线性MPC控制器时，可以预览当前和未来的轨迹。

Xopt = info.Xopt;Xref = [Xopt (2: p + 1,:); repmat (Xopt(最终,:),Tsteps-p, 1)];

使用nlmpcmove而且nlmpcmoveopt闭环仿真命令。

Hbar = waitbar(0，“模拟进展”）;选项= nlmpcmoveopt;为k = 1:步长利用传感器噪声获得植物输出测量值。yk = xHistory(k,1:3)' + randn*0.01;基于测量的正确状态估计。xk =正确(EKF, yk);使用参考预览计算控件移动。。[uk,options] = nlmpcmove(nlobj_tracking,xk,lastMV,Xref(k:min(k+9,Tsteps)，:)，[]，options);预测下一步的状态。预测(EKF、英国、Ts);存储控制移动和更新最后MV为下一步。uHistory(k，:) = uk';lastMV = uk;为下一步更新实际植物状态，解决%基于当前状态xk和输入uk的连续时间ode。ODEFUN = @(t,xk) FlyingRobotStateFcn(xk,uk);[吹捧,你]=数值([0 t], ODEFUN xHistory (k,:));保存状态值。xHistory(k+1，:) = YOUT(end，:);更新状态栏。waitbar (k / Tsteps百巴);结束关上(百巴)

将计划的闭环轨迹与实际的闭环轨迹进行比较。

FlyingRobotPlotTracking(信息、t、p Tsteps xHistory, uHistory);

实际油耗= 10.707288

非线性MPC反馈控制器成功地移动机器人(蓝色块)，遵循最优轨迹(黄色块)，并将其停在最后一个图中的最终位置(红色块)。

实际燃油成本高于计划成本。产生此结果的主要原因是，由于我们在反馈控制器中使用了较短的预测和控制范围，因此与规划阶段使用的优化问题相比，每个区间的控制决策都是次优的。

参考文献

[1] Y. Sakawa。利用最优控制进行自由飞行机器人的轨迹规划。最优控制应用与方法， Vol. 20, 1999, pp. 235-248。