飞行机器人

本例中的飞行机器人具有四个推进器将其左右移动在2 d的空间。该模型有六个州：

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

轨迹规划

机器人最初停留在(-10、-10)用的取向角π/ 2弧度（朝北）。这个例子的飞行动作是移动和在最后的位置停放机器人[0,0]以…的角度0弧度（面向东）在12秒。目标是找到最优路径，使推进器在机动过程中消耗的燃料总量最小化。

非线性MPC是轨迹规划问题的理想工具，因为它解决了由于目前植物状态的开环约束的非线性优化问题。随着非线性动力学模型的有效性，MPC可以做出更准确的决策。

创建一个非线性的MPC对象6状态，6输出，和4投入。默认情况下，所有的输入操作变量（MVS）。

为nx = 6;NY = 6;NU = 4;nlobj = nlmpc（NX，NY，NU）;

在标准的成本函数，零个权重是通过默认，因为有比的MV OVS少施加到一个或多个OVS。

指定使用函数名的预测模型状态的功能。您还可以指定使用功能手柄功能。有关状态功能的详细信息，打开FlyingRobotStateFcn.m。有关指定预测模型的详细信息，请参阅指定非线性MPC预测模型。

nlobj.Model.StateFcn =“FlyingRobotStateFcn”;

指定使用函数手柄的状态函数的雅可比。它是为预测模型提供了一个分析雅克比的最佳实践。这样做显著提高了仿真效率。关于雅可比功能的详细信息，打开FlyingRobotStateJacobianFcn.m。

nlobj.Jacobian。StateFcn = @FlyingRobotStateJacobianFcn;

对于这个例子中，目标预测时间12秒。因此，指定采样时间为0.4秒的预测范围三十脚步。

TS = 0.4;P = 30;nlobj.Ts = TS;nlobj.PredictionHorizo​​n = P;

为了进行轨迹规划，而不是反馈控制，使用的最大控制范围，它提供了最优化问题的决定变量的最大数量。由于轨迹规划通常比反馈控制器慢得多的采样率运行，由一个大的控制范围引入的额外计算量是可以容忍的。设置控制地平线等于预测水平。

nlobj。ControlHorizon = p;

轨迹规划问题通常涉及非线性成本函数，它可以用来寻找最短距离，最大的利润，或在这种情况下，最小的燃油消耗。因为推力值是燃料消耗的直接指标，计算所述燃料成本作为横跨预测范围中使用的推力值的总和。指定使用匿名函数处理这个成本函数。有关指定成本函数的详细信息，请参阅指定非线性MPC的成本函数。

nlobj.Optimization.CustomCostFcn = @（X，U，E，数据）的TS *总和（总和（U（1：P，:)））;

在这个例子中，自定义成本函数代替了典型的反馈控制使用的默认成本函数。

nlobj.Optimization.ReplaceStandardCost = TRUE;

机动的目标是将机器人停在[0,0]以…的角度0弧度在第12秒。指定这个目标上的状态，其中在最后一步预测每位置和速度状态应该是零等式约束。有关指定约束功能的详细信息，请参阅为非线性MPC指定约束。

nlobj.Optimization.CustomEqConFcn = @（X，U，数据）X（端，:)';

最好的做法是为您的定制成本和约束函数提供解析的雅可比函数。然而，这个例子故意跳过它们，使得非线性MPC控制器使用内置的数值扰动方法计算它们的雅可比矩阵。

每个推力具有之间的操作范围0和1，它被转换成MVs上的下界和上界。

对于CT = 1：NU nlobj.MV（克拉）.Min = 0;nlobj.MV（CT）最大= 1;结束

指定机器人的初始条件。

X0 = [-10 -10; PI / 2; 0; 0; 0];机器人停在[-10，-10]处，面朝北U0 =零（NU，1）;％推力为零

这是验证用户提供的模型，成本和约束功能及其雅克比的最佳实践。要做到这一点，使用validateFcns命令。

validateFcns（nlobj，X0，U0）;

Model.StateFcn是OK。Jacobian.StateFcn是OK。无输出函数指定。假设“Y = X”中的预测模型。Optimization.CustomCostFcn是OK。Optimization.CustomEqConFcn是OK。用户提供的模型，成本和约束函数的分析完成。

最优的状态和MV轨迹可以通过调用nlmpcmove命令一次，给定的当前状态X0和最后一个MVU0。最佳的成本和轨迹返回的部分信息输出参数。

[〜，〜，信息] = nlmpcmove（nlobj，X0，U0）;

Slack变量未使用或零加权在您的自定义成本函数。所有的约束都将是困难的。

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

FlyingRobotPlotPlanning(信息);

最佳燃料消耗= 7.797825

第一条曲线显示了在机动六个机器人状态的最佳轨迹。第二幅图显示了四个目标对应的最优MV轮廓。的第三曲线示出了机器人的X-Y位置轨迹，从移动(-10 -10π/ 2)至(0 0 0)。

对于路径跟踪反馈控制

最优轨迹被发现后，反馈控制器需要将机器人沿着该路径移动。从理论上讲，可以直接应用的最佳MV轮廓的推进器来实现前馈控制。然而，在实践中，需要反馈控制器拒绝干扰和补偿建模误差。

您能够使用不同的反馈控制技术跟踪。在这个例子中，你使用另一种非线性MPC控制器到机器人移动到最终位置。在此路径跟踪问题，跟踪所有六个州的引用。

nlobj_tracking = nlmpc（NX，NY，NU）;

在标准的成本函数，零个权重是通过默认，因为有比的MV OVS少施加到一个或多个OVS。

使用相同的状态的功能和它的行列功能。

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

对于反馈控制应用程序，通过指定更短的预测和控制范围来减少计算量。

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

在非线性MPC默认成本函数是适合于参考跟踪和干扰抑制的标准二次代价函数。对于跟踪，状态具有比MV移动更高的优先级（较大的权重处罚）。

nlobj_tracking.Weights.ManipulatedVariablesRate = 0.2 *一（1，NU）;nlobj_tracking.Weights.OutputVariables = 5点*一（1中，nx）;

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

对于CT = 1：NU nlobj_tracking.MV（克拉）.Min = 0;nlobj_tracking.MV（CT）最大= 1;结束

此外，为降低油耗，很明显，U1和U2不能在操作过程中随时阳性。因此，实现平等的限制，使得U（1）* U（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)];

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

validateFcns (nlobj_tracking x0,情况);

Model.StateFcn是OK。Jacobian.StateFcn是OK。无输出函数指定。假设“Y = X”中的预测模型。Optimization.CustomEqConFcn是OK。用户提供的模型，成本和约束函数的分析完成。

非线性状态估计

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

因为一个EKF需要离散时间模型，可以使用梯形规则自X（K）至x过渡（K + 1），这需要的溶液nx非线性代数方程组。欲了解更多信息，请打开FlyingRobotStateFcnDiscreteTime.m。

DStateFcn = @（XK，UK中，Ts）FlyingRobotStateFcnDiscreteTime（XK，UK，TS）;

测量可以帮助EKF纠正状态估计。只有前三个状态的信号。

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

创建EKF，并指示测量几乎没有噪音。

卡尔曼滤波器= extendedKalmanFilter (DStateFcn DMeasFcn, x0);卡尔曼滤波器。MeasurementNoise = 0.01;

跟踪控制的闭环仿真

对于模拟系统32步骤与正确的初始条件。

Tsteps = 32;xHistory = X0' ;uHistory = [];lastMV =零（NU，1）;

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

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

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

百巴= waitbar (0,“模拟进展”）;选项= nlmpcmoveopt;对于K = 1：Tsteps获得带有传感器噪声的工厂输出测量值。YK = xHistory（K，1：3）” + randn * 0.01;基于测量％正确的状态估计。xk =正确(EKF, yk);使用参考预览来计算控件移动。(英国、期权)= nlmpcmove (nlobj_tracking, xk, lastMV Xref (k: min (k + 9, Tsteps):),[],选项);％预测的状态用于下一步骤。预测（EKF，UK，TS）;％存储控制移动和更新的最后一个MV下一步。英国uHistory (k) = ';lastMV =英国;通过求解，更新下一步的实际植物状态基于当前状态xk和输入uk的连续时间ode。ODEFUN = @(t,xk) FlyingRobotStateFcn(xk，英国);[吹捧,你]=数值([0 t], ODEFUN xHistory (k,:));％的存储状态值。xHistory（K + 1，:) = YOUT（端，:)，更新状态栏。waitbar（K / Tsteps，HBAR）;结束关上(百巴)

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

FlyingRobotPlotTracking（资讯，TS，P，Tsteps，xHistory，uHistory）;

实际油耗= 10.756729

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

实际的燃料成本比计划成本较高。导致这一结果的主要原因是，由于我们在反馈控制器，在每个时间间隔控制决定是次优的相比，在规划阶段使用的优化问题，采用较短的预测和控制视野。

参考文献

[1] Y.佐川。“通过使用最优控制的自由飞行机器人的轨迹规划。”最优控制应用和方法， 1999年第20卷，第235-248页。