轨迹优化和控制使用非线性MPC的飞行机器人

这个示例使用:

打开脚本

这个例子展示了如何找到最优飞行轨迹,机器人从一个位置到另一个使用非线性MPC控制器以最小的燃料成本。此外,另一个非线性MPC控制器,以及一个扩展卡尔曼滤波器,驱动机器人以及闭环最优轨迹模拟。

飞行机器人

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

x (1)x惯性的质心坐标
x (2)y惯性的质心坐标
x (3)θ机器人(推力)方向
x (4)vx、速度(x)
x (5)v,速度
x (6)ω,θ的角速度

飞行机器人的更多信息,参见[1]。本文中的模型使用两个手臂从1到1。然而,这个例子假设有四个物理机器人的手臂,从0到1,达到相同的控制自由。

轨迹规划

机器人最初在休息(-10、-10)的定向角π/ 2弧度(面临北)。这个例子的机动飞行机器人移动和公园在最后的位置(0,0)的角0弧度(朝东)12秒。我们的目标是找到最优路径,这样的燃料消耗总量由推进器在机动最小化。

非线性MPC轨迹规划问题的理想工具,因为它解决了一个开环约束非线性优化问题在当前植物状态。非线性动态模型的可用性,MPC能够做出更准确的决策。

在这个例子中,目标预测时间12秒。因此,指定一个样本的时间0.4秒,预测地平线30.步骤。创建一个多级非线性MPC对象6州和4输入。默认情况下,所有的输入是操纵变量(MVs)。

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

路径规划问题,是典型的允许MPC自由移动预测在每个步骤中,它提供了最大数量的决策变量的优化问题。因为计划通常运行在一个慢得多采样率反馈控制器,所引入的额外计算量较大的优化问题可以被接受。

指定使用函数名预测模型状态函数。您还可以指定函数使用一个函数处理。细节的态函数,开放FlyingRobotStateFcn.m。指定预测模型的更多信息,请参阅为非线性MPC指定预测模型。

nlobj.Model。StateFcn =“FlyingRobotStateFcn”;

指定国家的雅可比矩阵函数使用一个函数处理。它是最佳实践提供一个分析预测模型的雅可比矩阵。这样做极大地提高了仿真效率。雅可比矩阵函数的详细信息,开放FlyingRobotStateJacobianFcn.m。

nlobj.Model。StateJacFcn = @FlyingRobotStateJacobianFcn;

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

为ct = 1: p nlobj.Stages (ct)。CostFcn =“FlyingRobotCostFcn”;结束

演习的目的是公园的机器人(0,0)的角0在12日的第二个弧度。这一目标指定为终端状态约束,位置和速度的每一个州在最后一步预测(p + 1)阶段应该是零。指定约束函数的更多信息,请参阅为通用非线性MPC指定约束。

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

提供分析雅可比矩阵函数是最佳实践阶段成本和约束功能。然而,这个例子故意跳过他们,这样计算雅可比矩阵的非线性MPC控制器使用内置的数值摄动方法。

每个推力之间的操作范围0和1,这是翻译成MVs上下界限。

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

指定机器人的初始条件。

x0 =(-10; -10;π/ 2,0,0,0);%机器人公园(-10、-10),面对北方情况= 0(ν,1);%的推力是零

最佳实践是验证用户提供的模型,成本,和约束函数和雅克比。为此,使用validateFcns命令。

validateFcns (nlobj x0,情况);

模型。StateFcn就可以了。模型。StateJacFcn就可以了。“CostFcn”下面的阶段(1 2 3 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情况。最优成本和轨迹的一部分返回信息输出参数。

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

情节最优轨迹。最优成本是7.8。

FlyingRobotPlotPlanning(信息,Ts);

最佳的燃油消耗= 7.797825

第一个情节显示机器人六州的最优轨迹机动。第二个图显示了相应的最佳MV简介四个手臂。第三个情节显示机器人的x - y位置轨迹,从(-10 -10π/ 2)来(0 0 0)。

路径跟踪的反馈控制

找到最优轨迹后,需要一个反馈控制器沿着路径移动机器人。从理论上讲,你可以直接应用的最佳MV概要文件推进器实现前馈控制。然而,在实践中,需要反馈控制器来抑制干扰和补偿建模错误。

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

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

零权重应用于一个或多个ov因为有MVs比ov少。

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

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:νnlobj_tracking.MV (ct)。最小值= 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就可以了。雅可比矩阵。StateFcn就可以了。没有指定的输出函数。假设“y = x”预测模型。优化。CustomEqConFcn就可以了。用户提供的模型分析、成本和约束函数完成。

非线性状态估计

在这个例子中,只有三位州(x, y和角度)测量。速度状态是无边无际的,必须估计。使用一个扩展卡尔曼滤波器(EKF)从控制系统工具箱™非线性状态估计。

因为一个卡尔曼滤波器需要一个离散时间模型,使用梯形法则从x (k) x (k + 1),这就需要的解决方案nx非线性代数方程。有关更多信息,开放FlyingRobotStateFcnDiscreteTime.m。

英国DStateFcn = @ (xk Ts) FlyingRobotStateFcnDiscreteTime(英国xk Ts);

测量可以帮助卡尔曼滤波器对其状态估计。只有前三个州测量。

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

创建卡尔曼滤波器,并表明,测量几乎没有噪音。

卡尔曼滤波器= 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命令闭环模拟。

百巴= 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、英国、Ts);%控制移动存储和更新的最后MV下一步。英国uHistory (k) = ';% #好< * SAGROW >lastMV =英国;%更新真正的植物为下一步解决%连续时间基于当前状态xk和输入英国的常微分方程。ODEFUN = @ (t, xk) FlyingRobotStateFcn(英国xk);[吹捧,你]=数值([0 t], ODEFUN xHistory (k,:));%存储的状态值。:xHistory (k + 1) =你(最终:);%更新状态栏。waitbar (k / Tsteps百巴);结束关上(百巴)

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

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

实际油耗= 10.707900

货币政策委员会的非线性反馈控制器成功地移动机器人(蓝色块),在最优轨迹(黄色块),和公园在最终位置(红色方块)在过去的人物。

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

确定一个神经飞行机器人系统状态空间模型

在工业应用中,有时很难手工推导非线性状态空间动态模型使用第一原理。另一个首要原则建模方法是黑箱建模基于实验数据。

本节将向您展示热训练一个神经网络近似一个状态函数,然后使用它作为非线性MPC的预测模型。这趋近于依赖idNeuralStateSpace对象,从系统辨识工具箱。

培训过程中实现支持文件金宝apptrainFlyingRobotNeuralStateSpaceModel。您可以使用它是一个简单的模板和修改它适合您的应用程序。大约需要10分钟才能完成,这取决于你的电脑。尝试它,分配DoTraining变量来真正的而不是假。

DoTraining = false;如果DoTraining nss = trainFlyingRobotNeuralStateSpaceModel;其他的负载nssModel.mat% #好< * UNRCH >结束

为预测生成M文件和使用它们

后idNeuralStateSpace系统培训,您可以自动生成MATLAB文件的状态函数及其雅可比矩阵分析函数使用generateMATLABFunction命令。

nssStateFcnName =“nssStateFcn”;generateMATLABFunction (nss nssStateFcnName);

使用神经网络模型作为预测模型在非线性MPC

生成的M文件与非线性MPC对象所需的接口兼容,因此,我们可以直接在非线性MPC对象中使用它们

nlobj_tracking.Model。StateFcn =nssStateFcnName; nlobj_tracking.Jacobian.StateFcn = [nssStateFcnName的雅可比矩阵];

再次运行模拟神经状态空间预测模型