基于非线性模型预测控制的四旋翼飞行器控制

这个例子使用了:

打开实时脚本

本例介绍了如何利用非线性模型预测控制(MPC)设计四旋翼飞行器的轨迹跟踪控制器。

Quadrotor模型

四旋翼飞行器有四个向上的旋翼。从四旋翼的质心，转子被放置在一个正方形的形成与相等的距离。基于欧拉-拉格朗日方程[1]建立了四旋翼飞行器动力学数学模型。

四旋翼机的十二个状态是:

$［ x ， y ， z ， ϕ ， θ ， ψ ， x_{}^{˙} ， y_{}^{˙} ， z_{}^{˙} ， ϕ_{}^{˙} ， θ_{}^{˙} ， ψ_{}^{˙} ］$ ，

在哪里

$［ x ， y ， z ］$ 表示惯性系中的位置
角位置 $［ ϕ ， θ ， ψ ］$ 是按横摇，俯仰和偏航的顺序
剩下的状态是位置和角度的速度

四旋翼的控制输入是四个旋翼角速度的平方:

$［ ω_{1}^{2} ， ω_{2}^{2} ， ω_{3.}^{2} ， ω_{4}^{2} ］$ 。

这些控制输入在身体的方向上产生力、扭矩和推力 $z$ 设在。在这个例子中，每个状态都是可测量的，并且控制输入被限制在[0,12]之内。 ${（ \frac{rad}{年代} ）}^{2}$ 。

模型的状态函数和状态雅可比函数是用Symbolic Math Toolbox™软件建立和推导的。

getQuadrotorDynamicsAndJacobian;

ans =Function_handle with value:@QuadrotorStateFcn

ans =Function_handle with value:@QuadrotorStateJacobianFcn

的getQuadrotorDynamicsAndJacobian脚本生成以下文件:

QuadrotorStateFcn.m-状态函数
QuadrotorStateJacobianFcn.m-状态雅可比函数

有关任何一个函数的详细信息，请打开相应的文件。

非线性模型预测控制器设计

创建一个具有12个状态、12个输出的非线性MPC对象4输入。默认情况下，所有输入都是操作变量(mv)。

Nx = 12;Ny = 12;Nu = 4;Nlobj = nlmpc(nx, ny, nu);

在标准代价函数中，默认情况下对一个或多个ov应用零权重，因为mv比ov少。

使用函数名指定预测模型状态函数。也可以使用函数句柄指定函数。

nlobj.Model.StateFcn =“QuadrotorStateFcn”；

使用函数句柄指定状态函数的雅可比矩阵。最好的做法是为预测模型提供一个解析雅可比矩阵。这样做可以显著提高模拟效率。

nlobj.Jacobian.StateFcn = @QuadrotorStateJacobianFcn;

验证您的预测模型、自定义函数及其雅可比矩阵。

rng (0) validateFcns (nlobj,兰德(nx, 1),兰特(ν,1));

模型。StateFcn是OK的。雅可比矩阵。StateFcn是OK的。没有指定输出函数。假设预测模型中“y = x”。分析了用户提供的模型、成本和约束函数。

的采样时间0．1秒，预测水平18步，控制水平2步。

Ts = 0.1;P = 18;M = 2;nlobj。Ts = Ts;nlobj。PredictionHorizon = p;nlobj。ControlHorizon = m;

将所有四个控制输入限制在[0,12]范围内。

nlobj。MV = struct(“最小值”, {0, 0, 0, 0},“马克斯”,{12、12、12;12});

非线性MPC的缺省代价函数是一个适用于参考跟踪和干扰抑制的标准二次代价函数。在这个例子中，前6个状态 $［ x ， y ， z ， ϕ ， θ ， ψ ］$ 都需要遵循给定的参考轨迹。由于mv的数量(4)小于参考输出轨迹的数量(6)，因此没有足够的自由度来跟踪所有输出变量(OVs)的期望轨迹。

nlobj.Weights.OutputVariables = [1 1 1 1 1 1 0 0 0 0 0 0];

在这个例子中，MV也有名义目标来保持四旋翼漂浮，这可能导致MV和OV参考跟踪目标之间的冲突。若要对目标进行优先级排序，可将平均MV跟踪优先级设置为低于平均OV跟踪优先级。

nlobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];

此外，通过为MV变化率指定调优权重来惩罚激进的控制动作。

nlobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1 0.1];

闭环仿真

用目标轨迹模拟系统20秒。

指定初始条件X = [7;-10;0;0;0;0;0;0;0;%保持四旋翼浮动的名义控制Nloptions = nlmpcmoveopt;nloptions。MVTarget = [4.9 4.9 4.9 4.9];mv = nloptions.MVTarget;

仿真闭环系统nlmpcmove函数指定模拟选项nlmpcmove对象。

持续时间= 20;Hbar = waitbar(0，“模拟进展”）;xHistory = x';lastMV = mv;uHistory = lastMV;为k = 1:(Duration/Ts)设置预览的引用t = linspace(k*Ts， (k+p-1)*Ts,p);yref = QuadrotorReferenceTrajectory(t);使用参考预览计算控件移动。。xk = xHistory(k，:);[uk,nloptions,info] = nlmpcmove(nlobj,xk,lastMV,yref'，[]，nloptions);uHistory(k+1，:) = uk';lastMV = uk;%更新状态。ODEFUN = @(t,xk) QuadrotorStateFcn(xk,uk);[吹捧,你]=数值([0 t], ODEFUN xHistory (k,:));xHistory(k+1，:) = YOUT(end，:);waitbar (k * Ts /持续时间,百巴);结束关上(百巴)

可视化和结果

绘制结果，并将计划的闭环轨迹与实际轨迹进行比较。

plotQuadrotorTrajectory;

图控制输入包含4个轴对象。轴对象1的标题输入1包含2个对象类型的楼梯，线。这些对象代表实际的、参考的。轴对象2的标题输入2包含2个对象类型的楼梯，线。这些对象代表实际的、参考的。轴对象3的标题输入3包含2个对象类型的楼梯，线。这些对象代表实际的、参考的。轴对象4的标题输入4包含2个对象类型的楼梯，线。这些对象代表实际的、参考的。