飞行机器人

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

有关飞行机器人的更多信息，请参阅[1]。纸张中的模型使用从-1到1的两个推力。然而，该示例假设机器人中有四个物理推进器，从0到1之间，实现相同的控制自由度。

轨迹规划

机器人最初休息(-10、-10)它的方向角是π/ 2弧度(面临北)。这个例子的飞行策略是将机器人移动并停放在最终的位置[0,0]角度为0弧度（面向东）12秒。目标是找到最佳路径，使得在机动期间推动器消耗的燃料总量最小化。

非线性MPC是轨迹规划问题的理想工具，因为它解决了当前工厂状态的开环受限的非线性优化问题。随着非线性动态模型的可用性，MPC可以做出更准确的决策。

在本例中，目标预测时间为12秒。因此，指定的采样时间为0．4秒和预测视界30.步骤。创建一个多阶段非线性MPC对象6州和4输入。默认情况下，所有输入都是被操纵变量（MV）的。

t = 0.4;p = 30;nx = 6;ν= 4;nlobj = nlmpcMultistage (p nxν);nlobj。Ts = t;

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

使用函数名指定预测模型状态函数。你也可以使用函数句柄来指定函数。有关状态功能的详细信息请参见“打开”flyingrobotstatefcn.m.．有关指定预测模型的更多信息，请参阅指定非线性MPC的预测模型．

nlobj.Model.StateFcn =“FlyingRobotStateFcn”；

使用函数句柄指定状态函数的Jacobian。最好的做法是为预测模型提供分析雅加索。这样做明显提高了模拟效率。有关雅可比功能的详细信息，请打开FlyingRobotStateJacobianFcn.m．

nlobj.Model.StateJacFcn = @FlyingRobotStateJacobianFcn;

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

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

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

nlobj.model.terminalstate =零（6,1）;

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

每个推力都有一个工作范围0和1，转换成mv的上界和下界。

为ct = 1:nu nlobj.MV(ct)。最小值= 0;nlobj.MV (ct)。Max = 1;结尾

为机器人指定初始条件。

x0 =(-10; -10;π/ 2,0,0,0);%机器人泊位在[-10，-10]，面朝北情况= 0(ν,1);％推力为零

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

validatefcns（nlobj，x0，u0）;

模型。StateFcn就可以了。模型。StateJacFcn就可以了。以下阶段的“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]都可以。分析了用户提供的模型、成本和约束功能。

最优状态和MV轨迹可以通过调用nlmpcmove命令一次，给定当前状态x0最后MV情况．最佳成本和轨迹作为其中的一部分返回信息输出参数。

[〜，〜，Info] = nlmpcmove（nlobj，x0，u0）;

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

FlyingRobotPlotPlanning(信息,Ts);

最佳油耗= 7.797825

第一个图显示了六种机器人状态在机动过程中的最优轨迹。第二幅图显示了四个推力对应的最优MV剖面。第三个图显示了机器人的X-Y位置轨迹，从(-10 -10π/ 2)来(0 0 0)．

路径跟踪的反馈控制

在找到最佳轨迹之后，需要反馈控制器沿路径移动机器人。理论上，您可以将最佳MV轮廓直接应用于推进器以实现前馈控制。然而，在实践中，需要反馈控制器来拒绝干扰并补偿建模错误。

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

纽约= 6;nlobj_tracking = nlmpc (nx、纽约、ν);

在标准成本函数中，默认情况下，零重量施加到一个或多个OV，因为没有比OVS更少。

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

nlobj_tracking.model.statefcn = nlobj.model.statefcn;nlobj_tracking.jacobian.statefcn = nlobj.model.statejacfcn;

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

nlobj_tracking.ts = ts;nlobj_tracking.predictionhorizo​​n = 10;nlobj_tracking.controlhorizo​​n = 4;

非线性MPC中的默认成本函数是适用于参考跟踪和干扰抑制的标准二次成本函数。为了跟踪，跟踪错误的优先级更高（输出上的较大罚款），而不是控制工作（MV速率上的较小罚款）。

nlobj_tracking.weights.manipuldvariablesrate = 0.2 *那些（1，nu）;nlobj_tracking.weights.outputvariables = 5 * oon（1，nx）;

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

为ct = 1:nu nlobj_tracking.MV(ct)。最小值= 0;nlobj_tracking.MV (ct)。Max = 1;结尾

此外，为了减少燃料消耗，很明显u1和u2在操作期间的任何时间都不能是积极的。因此，实现平等约束U（1）* U（2）必须0所有预测步骤。适用类似的约束u3和的愉快．

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,情况);

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

非线性状态估计

在这个例子中，只测量了三个位置状态(x, y和角度)。速度状态是无法测量的，必须估计。使用控制系统工具箱中的扩展卡尔曼滤波器(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，并表明测量结果很小。

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

跟踪控制的闭环仿真

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

Tsteps = 32;xHistory = x0 ';uHistory = [];lastMV = 0(ν,1);

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

xopt = info.xopt;xref = [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 Xref (k: min (k + 9, Tsteps):),[],选项);%预测下一步的状态。预测（EKF，UK，TS）;％存储控制移动并更新下一步的最后一个MV。英国uHistory (k) = ';lastMV =英国;更新真实的植物状态为下一步解决基于当前状态XK和输入英国的％连续时间杂物。ODEFUN = @(t,xk) FlyingRobotStateFcn(xk,uk);[吹捧,你]=数值([0 t], ODEFUN xHistory (k,:));%存储状态值。: xHistory (k + 1) =你(最终:);％更新状态栏。waitbar (k / Tsteps百巴);结尾关上(百巴)

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

Flyingrobotplottracking（信息，Ts，p，tsteps，xhistory，uhistory）;

实际油耗= 10.709242

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

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

参考文献

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