这个例子展示了如何使用非线性MPC控制器,以最小的燃料成本找到飞行机器人从一个位置到另一个位置的最优轨迹。此外,在闭环仿真中,另一种非线性MPC控制器与扩展卡尔曼滤波器一起驱动机器人沿最优轨迹运动。
这个例子中的飞行机器人有四个推进器,可以在二维空间中移动。该模型有六种状态:
x (1)x
质心的惯性坐标
x (2)y
质心的惯性坐标
x (3)θ
,机器人(推力)方向
x (4)vx
, x的速度
x (5)v
, y的速度
x (6)ω
,角速度
有关飞行机器人的更多信息,请参阅[1]。本文中的模型使用了两个推力,范围从-1到1。然而,这个例子假设机器人有四个物理推力,范围从0到1,以实现相同的控制自由度。
机器人最初停留在(-10、-10)
的方向角π/ 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 = nlmpcMultistage(p,nx,nu);nlobj。Ts = Ts;
对于路径规划问题,通常允许MPC在每个预测步骤都有自由移动,这为优化问题提供了最大数量的决策变量。由于规划通常以比反馈控制器慢得多的采样率运行,因此可以接受由较大的优化问题引入的额外计算负载。
使用函数名指定预测模型状态函数。还可以使用函数句柄指定函数。有关状态函数的详细信息,请打开FlyingRobotStateFcn.m
。有关指定预测模型的详细信息,请参见指定非线性MPC的预测模型。
nlobj.Model.StateFcn =“FlyingRobotStateFcn”;
使用函数句柄指定状态函数的雅可比矩阵。最好的做法是为预测模型提供一个解析雅可比矩阵。这样做可以显著提高仿真效率。有关雅可比函数的详细信息,请打开FlyingRobotStateJacobianFcn.m
。
nlobj. model . statejacianfcn = @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
,转化为mv的下界和上界。
为ct = 1:nu nlobj.MV(ct)。Min = 0;nlobj.MV (ct)。Max = 1;结束
指定机器人的初始条件。
X0 = [-10;-10;pi/2;0;0;0];机器人停在[-10,-10]处,面朝北U0 = 0 (nu,1);%推力为零
最佳实践是验证用户提供的模型、成本和约束函数及其雅可比矩阵。要做到这一点,使用validateFcns
命令。
validateFcns (nlobj x0,情况);
模型。StateFcn是OK。模型。statjacfcn没问题。以下阶段的“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]是OK的。分析用户提供的模型,成本和约束功能完成。
最优状态和MV轨迹可以通过调用nlmpcmove
命令一次,给定当前状态x0
最后一个MV情况
。最优成本和轨迹将作为模型的一部分返回信息
输出参数。
[~,~,info] = nlmpcmove(nlobj, 0, 0);
绘制最优轨迹。最优成本为7.8。
FlyingRobotPlotPlanning(信息,Ts);
最优油耗= 7.797825
第一张图显示了机器人在机动过程中六种状态的最优轨迹。第二张图显示了四个推力对应的最佳MV曲线。第三张图显示了机器人的X-Y位置轨迹,从[-10 -10 pi/2]
来[00 00 00]
。
找到最优轨迹后,需要一个反馈控制器使机器人沿着路径移动。理论上,您可以直接将最优MV剖面应用于推进器以实现前馈控制。然而,在实践中,需要一个反馈控制器来抑制干扰和补偿建模误差。
您可以使用不同的反馈控制技术进行跟踪。在本例中,您使用通用非线性MPC控制器将机器人移动到最终位置。在这个路径跟踪问题中,要跟踪所有六种状态的引用(输出的数量等于状态的数量)。
Ny = 6;Nlobj_tracking = nlmpc(nx,ny,nu);
在标准代价函数中,由于mv比OVs少,默认情况下对一个或多个OVs应用零权重。
使用相同的状态函数和它的雅可比函数。
nlobj_tracking.Model。StateFcn = nlobj.Model.StateFcn;nlobj_tracking.Jacobian。StateFcn = nlobj. model . statjacfcn;
对于跟踪控制应用程序,通过指定较短的预测范围(不需要展望未来)和控制范围(例如,在前几个预测步骤分配自由移动)来减少计算工作量。
nlobj_tracking。Ts = Ts;nlobj_tracking。PredictionHorizon = 10;nlobj_tracking。ControlHorizon = 4;
非线性MPC的默认代价函数是一个标准的二次代价函数,适用于参考跟踪和干扰抑制。对于跟踪,跟踪错误的优先级(输出的惩罚权重较大)高于控制努力(MV速率的惩罚权重较小)。
nlobj_tracking.Weights。manipulatedvariablerate = 0.2*ones(1,nu);nlobj_tracking.Weights。输出变量= 5*ones(1,nx);
为推进器输入设置相同的边界。
为ct = 1:nu nlobj_tracking.MV(ct)Min = 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是OK。雅可比矩阵。StateFcn是OK。没有指定输出函数。假设预测模型中的y = x。优化。customeqconf是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, 0);卡尔曼滤波器。噪声= 0.01;
模拟系统32
具有正确初始条件的步骤。
Tsteps = 32;xHistory = 0';uHistory = [];lastMV = 0 (nu,1);
参考信号是在规划阶段计算得到的最优状态轨迹。当将这些轨迹传递给非线性MPC控制器时,可以预览当前和未来的轨迹。
Xopt = info.Xopt;Xref = [Xopt (2: p + 1,:); repmat (Xopt(最终,:),Tsteps-p, 1)];
使用nlmpcmove
和nlmpcmoveopt
闭环仿真命令。
Hbar = waitbar(0;“模拟进展”);Options = nlmpcmoveopt;为k = 1:Tsteps获得带有传感器噪声的工厂输出测量值。yk = xHistory(k,1:3)' + randn*0.01;%基于测量的正确状态估计。xk = correct(EKF, yk);使用参考预览计算控件移动。[uk,options] = nlmpcmove(nlobj_tracking,xk,lastMV,Xref(k:min(k+9,Tsteps),:),[],options);%预测下一步的状态。预测(EKF、英国、Ts);存储控制移动,并为下一步更新最后一个MV。uHistory(k,:) = uk';lastMV = uk;通过求解。来更新下一步的实际工厂状态%基于当前状态xk和输入uk的连续时间ode。ODEFUN = @(t,xk) FlyingRobotStateFcn(xk,uk);[吹捧,你]=数值([0 t], ODEFUN xHistory (k,:));%存储状态值。xHistory(k+1,:) = YOUT(end,:);%更新状态栏。waitbar (k / Tsteps百巴);结束关上(百巴)
比较计划和实际的闭环轨迹。
FlyingRobotPlotTracking(信息、t、p Tsteps xHistory, uHistory);
实际燃油消耗量= 10.709242
非线性MPC反馈控制器成功移动机器人(蓝色块),沿着最优轨迹(黄色块),并将其停在最后一个图中的最终位置(红色块)。
实际燃料成本高于计划成本。产生这一结果的主要原因是,由于我们在反馈控制器中使用了较短的预测和控制范围,因此与规划阶段使用的优化问题相比,每个区间的控制决策都是次优的。
b[1] Y.坂川。“基于最优控制的自由飞行机器人轨迹规划”。最优控制应用与方法, 1999年第20卷,第235-248页。