飞行机器人

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

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

轨迹规划

机器人最初搁在[-10，-10]用的取向角Pi / 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.ControlHorizo​​n = 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，其被翻译成上的MV下界和上界。

对于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）;

松弛变量未使用或在您的自定义成本函数零加权。所有的约束将很难。

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

FlyingRobotPlotPlanning（信息）;

最佳燃料消耗= 7.797825

第一条曲线显示了在机动六个机器人状态的最佳轨迹。第二幅图所示为四个目标对应的最优MV轮廓。的第三曲线示出了机器人的X-Y位置轨迹，从移动[-10 -10 PI / 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 = TS;nlobj_tracking.PredictionHorizo​​n = 10;nlobj_tracking.ControlHorizo​​n = 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：结束-1,1）* U（1：结束-1,2）。。U（1：结束-1,3）* U（1：结束-1,4）]。

验证您的预测模型和自定义功能，其雅可比。

validateFcns（nlobj_tracking，X0，U0）;

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，并指示测量几乎没有噪音。

EKF = extendedKalmanFilter（DStateFcn，DMeasFcn，X0）;EKF.MeasurementNoise = 0.01;

跟踪控制的闭环仿真

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

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

该参考信号是在规划阶段中计算出的最佳状态的轨迹。当通过这些轨迹非线性MPC控制器，在当前和未来的轨迹可以进行预览。

Xopt = info.Xopt;外部参照= [Xopt（2：P + 1，:); repmat（Xopt（端，:)，Tsteps-P，1）];

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

HBAR = waitbar（0，“模拟研究进展”）;选项= nlmpcmoveopt;对于K = 1：Tsteps％获得植物输出测量结果与传感器噪声。YK = xHistory（K，1：3）” + randn * 0.01;基于测量％正确的状态估计。XK =正确（EKF，YK）;％计算参考预览控制移动。[英国，选项] = nlmpcmove（nlobj_tracking，XK，lastMV，外部参照（K：分第（k + 9，Tsteps）：），[]，选项）;％预测的状态用于下一步骤。预测（EKF，UK，TS）;％存储控制移动和更新的最后一个MV下一步。uHistory（K，:) = UK';lastMV = UK;％更新在实际设备状态通过求解下一步基于当前状态％连续时间常微分方程XK和输入英国。ODEFUN = @（T，XK）FlyingRobotStateFcn（XK，UK）;[TOUT，YOUT] = ODE45（ODEFUN，[0 TS]，xHistory（K，:)'）;％的存储状态值。xHistory（K + 1，:) = YOUT（端，:);％更新状态栏。waitbar（K / Tsteps，HBAR）;结束接近（HBAR）

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

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

实际油耗= 10.756729

非线性MPC反馈控制器成功移动机器人（蓝色块），以下的最佳轨迹（黄色块），并在最后一个图中的最终位置（红色块）公园它。

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

参考

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