飞行机器人

本例中的飞行机器人有四个推进器，可以在二维空间中移动。该模型有六种状态：

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

轨迹规划

机器人最初停留在[-10,-10]方位角为pi/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=nlmpc多级（p，nx，nu）；nlobj.Ts=Ts；

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

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

nlobj.Model.StateFcn=“飞行机器人状态FCN”;

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

nlobj.Model.StateJacFcn=@FlyingRobotStateJacobianFcn；

轨迹规划问题通常涉及一个非线性成本函数，该函数可用于寻找最短距离、最大利润，或在这种情况下，最小油耗。由于推力值是油耗的直接指标，因此计算燃料成本，作为每个预测阶段推力值的总和1至阶段p。使用命名函数指定此成本函数。有关指定成本函数的详细信息，请参阅为非线性MPC指定成本函数．

为ct=1:p非手术分期（ct）=“飞行机器人成本”;结束

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

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

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

每个推力都有一个操作范围0和1.，将其转换为MVs上的下限和上限。

为ct=1:nu nlobj.MV（ct）.Min=0；nlobj.MV（ct）.Max=1；结束

指定机器人的初始条件。

x0=[-10；-10；pi/2；0；0；0]；%机器人停在[-10，-10]，面向北方u0=零（nu，1）；推力百分比为零

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

validateFcns (nlobj x0,情况);

Model.StateFcn正常。Model.StateJacFcn正常。以下阶段的“成本控制”是可以的[1 2 3 4 6 7 8 9 10 11 12 14 15 16 18 19 20 22 23 24 25 26 27 28 29 30]。分析用户提供的模型、成本和约束功能。

通过调用nlmpcmove命令一次，给定当前状态x0最后一个MVu0．最优成本和轨迹作为信息输出参数。

[~, ~,信息]= nlmpcmove (x0, nlobj情况);

绘制最佳轨迹。最佳成本为7.8。

飞行机器人绘图规划（信息，Ts）；

最佳油耗=7.797825

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

路径跟踪的反馈控制

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

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

ny=6；nlobj_跟踪=nlmpc（nx，ny，nu）；

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

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

nlobj_tracking.Model。StateFcn = nlobj.Model.StateFcn;nlobj_tracking.Jacobian。StateFcn = nlobj.Model.StateJacFcn;

对于跟踪控制应用，通过指定较短的预测范围（无需展望未来）和控制范围（例如，在最初的几个预测步骤中分配自由移动）来减少计算工作量。

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

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

nlobj_tracking.Weights。ManipulatedVariablesRate = 0.2 * 1(ν);nlobj_tracking.Weights。OutputVariables = 5 *的(nx);

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

为ct=1:nu nlobj_tracking.MV（ct）.Min=0；nlobj_跟踪.MV（ct）.Max=1；结束

此外，为了降低燃油消耗，很明显u1和u2在手术期间任何时候都不能阳性。因此，实现相等约束u (1) * (2)必须是0用于所有预测步骤。应用类似的约束u3和u4．

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)];

验证预测模型和自定义函数及其雅可比矩阵。

验证EFCNS（nlobj_跟踪，x0，u0）；

Model.StateFcn正常。Jacobian.StateFcn没问题。没有指定输出函数。假设预测模型中的“y=x”。Optimization.CustomEqConFcn正常。分析用户提供的模型、成本和约束功能。

非线性状态估计

在此示例中，仅测量三个位置状态（x、y和角度）。速度状态无法测量，必须进行估计。使用控制系统工具箱中的扩展卡尔曼滤波器（EKF）™ 用于非线性状态估计。

因为EKF需要离散时间模型，所以使用梯形规则从x（k）过渡到x（k+1），这需要nx非线性代数方程组。有关更多信息，请打开FlyingRobotStateFcnDiscreteTime.m．

DStateFcn=@（xk，英国，Ts）飞行机器人状态fcndiscreteTime（xk，英国，Ts）；

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

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

创建EKF，并表明测量结果有很小的噪声。

EKF=扩展卡尔曼滤波器（DStateFcn，DMeasFcn，x0）；EKF.MeasurementNoise=0.01；

跟踪控制的闭环仿真

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

t步骤=32；xHistory=x0'；历史=[]；lastMV=零（nu，1）；

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

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

使用nlmpcmove和nlmpcmoveopt闭环仿真命令。

百巴= waitbar (0,“模拟进展”）;选择= nlmpcmoveopt;为k=1:t步骤%利用传感器噪声获取电厂输出测量值。yk=xHistory（k，1:3）“+randn*0.01；%根据测量值进行正确的状态估计。xk=正确（EKF，yk）；%使用参照预览计算控件移动。[uk，选项]=nlmpcmove（nlobj_跟踪，xk，lastMV，外部参照（k:min（k+9，Tsteps），：），[]，选项）；%预测下一步的状态。预测(EKF、英国、Ts);%存储控制移动并更新最后一个MV为下一步。uHistory（k，：）=uk’；lastMV=uk；%通过解决以下问题，更新下一步的实际电厂状态：%连续时间ode基于当前状态xk和输入uk。ODEFUN=@（t，xk）FlyingRobotStateFcn（xk，英国）；[TOUT，YOUT]=ode45（ODEFUN[0 Ts]，xHistory（k，：））；%存储状态值。xHistory（k+1，：）=YOUT（end，：）；%更新状态栏。waitbar（k/T步骤，hbar）；结束关闭（hbar）

比较计划和实际闭环轨迹。

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

实际燃油消耗量=10.709242

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

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

工具书类

[1] y Sakawa。"利用最优控制实现自由飞行机器人的轨迹规划"最优控制应用与方法，第20卷，1999年，第235-248页。