概述

一个MPC控制器使用一个内部的模型来预测植物的行为。鉴于电站的当前状态,货币政策委员会在预测模型的基础上,找到一个最优控制序列,最大限度地减少成本和满足在指定的约束预测地平线。因为MPC发现植物的未来轨迹的同时,它可以作为一个强大的工具来解决轨迹优化问题,如自动停车的车辆和机械臂的运动计划。

在这样的轨迹优化问题工厂,成本函数和约束往往是非线性的。因此,您需要使用非线性MPC控制器问题制定和解决方案。在本例中,您设计一个非线性MPC控制器,找到一个最优路线自动公园一辆卡车与一个拖车从初始位置到目标位置,这是两个静态障碍。然后您可以生成的路径传递给底层控制器作为参考信号,以便执行实时停车操作。

这个例子需要优化工具箱™和机器人系统工具箱™软件。

卡车和拖车系统

下面的图显示了卡车和拖车非线性动态系统。

这个模型的状态是:

该模型的输入:

对于这个模型,在米长度和位置,速度是在m / s,和角弧度。

定义下面的模型参数。

params =结构(“米”, 1…“L1”6…“W1”,2.5,…“外语”10…“那”,2.5,…“Lwheel”,1…“Wwheel”,0.4);

非线性模型的实现TruckTrailerStateFcn雅可比矩阵函数及其manually-derived分析(用于加快优化)的TruckTrailerStateJacobianFcn函数。

在本例中,由于您使用货币政策委员会作为路径规划,而不是低级路径跟踪控制器,卡车的纵向速度作为一个操纵变量而不是加速。

自动停车问题

停车场有100米宽,60米长。我们的目标是找到一个可行的路径,卡车和拖车系统从初始位置到目标位置的绿十字(下图)在20秒使用反向停车。在这个过程中,计划的路径必须避免碰撞停车位旁边有两个障碍。

initialPose = (0, 0, 0, 0);targetPose =[0; -25;π/ 2 0];TruckTrailerPlot (initialPose targetPose params);

最初的植物输入(转向角和纵向速度)0。

情况= 0 (2,1);

初始位置必须是有效的。使用不等式约束函数TruckTrailerIneqConFcn检查的有效性。这个函数的细节将在下一节中讨论。碰撞检测是由特定的机器人系统工具箱函数。

cineq = TruckTrailerIneqConFcn (1 initialPose情况,…[params.M; params.L1 params.L2; params.W1 params.W2]);如果任何(cineq > 0)流(“初始姿势无效。\ n”);返回结束

使用多级非线性MPC路径规划

相比之下,通用非线性MPC控制器(nlmpc对象),多级非线性MPC为你提供了一个更加灵活和有效的方式来实现货币政策委员会举行了成本和约束。这种灵活性和效率对轨迹规划尤其有用。

一个多级非线性MPC控制器与预测地平线p定义了p + 1阶段,代表时间k(当前时间),k + 1、……k + p。对于每一个阶段,你可以指定stage-specific成本、不等式约束和等式约束的功能。这些函数仅依赖于植物状态和输入值。鉴于目前植物状态x [k],MPC发现操纵变量(MV)轨迹(从时间k来k + p - 1)优化总结阶段(从时间成本k来k + p舞台),满足所有约束条件(从时间k来k + p)。

在这个例子中,植物有四个州和两个输入(MVs)。选择预测地平线p和样品时间Ts这样p * t = 20。

创建多级非线性MPC控制器。

p = 40;nlobj = nlmpcMultistage (p, 4, 2);nlobj。Ts= 0.5;

指定的预测模型及其雅可比矩阵分析控制器对象。由于模型需要三个参数(米,L1,L2),设置Model.ParameterLength来3。

nlobj.Model。StateFcn =“TruckTrailerStateFcn”;nlobj.Model。StateJacFcn =“TruckTrailerStateJacobianFcn”;nlobj.Model。ParameterLength = 3;

指定两个操纵变量困难的界限。操舵角必须保持在+ / - 45度。最大的前进速度是10 m / s和最大反向速度是10 m / s。

nlobj.MV (1)。Min = -π/ 4;%最小转向角nlobj.MV (1)。Max =π/ 4;%最大转向角nlobj.MV (2)。最小值= -10;%的最低速度(反向)nlobj.MV (2)。Max = 10;%最大速度(向前)

指定硬边界第四状态,卡车和拖车之间的角度。它不能超越+ / -90度由于力学的局限性。

nlobj.States (4)。Min = -π/ 2;nlobj.States (4)。Max =π/ 2;

你可以用不同的方式定义成本方面。例如,您可能想要减少时间、燃料消耗、速度或停车。对于这个示例,减少停车速度二次形式促进安全。

由于MVs只从阶段1阶段有效p,你不需要定义任何阶段成本在过去的阶段,p + 1。五个模型的设置,米,L1,L2,W1,W阶段1阶段参数吗p和利用不等式约束函数。

为ct = 1: p nlobj.Stages (ct)。CostFcn =“TruckTrailerCost”;nlobj.Stages (ct)。CostJacFcn =“TruckTrailerCostGradientFcn”;nlobj.Stages (ct)。ParameterLength = 5;结束

您可以使用不等式约束以避免在停车过程中碰撞。使用TruckTrailerIneqConFcn函数检查卡车或拖车是否碰撞与任何在一个特定阶段的两个静态障碍。当卡车或拖车周围1米内安全区障碍,碰撞检测。

一般来说,检查这种碰撞的预测步骤(从第二阶段阶段p + 1)。然而,在本例中,您只需要从2到检查阶段p因为最后阶段已经照顾的等式约束函数。

对于这个示例,不提供一个分析的雅可比矩阵不等式约束函数,因为它太复杂的手工推导。因此,控制器使用有限差分方法(数值摄动)在运行时估计雅可比矩阵。

为ct = 2: p nlobj.Stages (ct)。IneqConFcn =“TruckTrailerIneqConFcn”;结束

使用终端状态的最后阶段,以确保成功的停车在目标位置。在这个例子中,目标位置作为一个运行时提供信号。这里我们使用一个虚拟有限值让MPC知道哪个国家会在运行时目的性价值。

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

年底的多级非线性MPC设计,您可以使用validateFcns与随机初始植物状态和输入命令检查是否任何用户定义的状态,成本,和约束函数以及任何分析雅可比矩阵函数,有一个难题。

你必须提供所有的态函数定义和舞台在运行时参数控制器。StageParameter叠成一个包含所有阶段的参数向量。我们也使用TerminalState在运行时指定终端状态。

simdata = getSimulationData (nlobj,“TerminalState”);simdata。StateFcnParameter = [params.M; params.L1 params.L2];simdata。StageParameter = repmat ([params.M; params.L1 params.L2; params.W1; params.W2], p, 1);simdata。TerminalState = targetPose;validateFcns (nlobj [2、3、0.5; 0.4], [0.1, 0.2], simdata);

模型。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 31 32 33 34 35 36 37 38 39 40]就可以了。“CostJacFcn”下面的阶段(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 31 32 33 34 35 36 37 38 39 40]就可以了。“IneqConFcn”下面的阶段(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 31 32 33 34 35 36 37 38 39 40]就可以了。用户提供的模型分析、成本和约束函数完成。

由于默认非线性规划求解器进行求解fmincon搜索一个局部最小值,您必须提供一个很好的初始估计的决策变量,尤其是对轨迹优化问题通常涉及到一个复杂的空间(可能非凸)的解决方案。

这个例子中有244个决策变量,植物状态和输入总计(6)的第一个p(40)阶段,植物状态(4)在过去的阶段p + 1。的TruckTrailerInitialGuess函数使用简单的启发式方法产生的初始猜测。例如,如果上面的卡车和拖车最初定位障碍,它生成一个初始猜测一个路标;否则,它使用两个锚点(一个路标的旅行路线上的一个中间点的课程改变)。最初的猜测是显示为点在以下动画情节。

[simdata。InitialGuess XY0] = TruckTrailerInitialGuess (initialPose, targetPose情况p);

轨迹规划和仿真结果

使用nlmpcmove函数来找到最佳的停车路径,通常需要10到20秒,这取决于初始位置。

流(“自动停车规划运行…\ n”);抽搐;[~,~,信息]= nlmpcmove (nlobj、initialPose情况,simdata); t = toc;流(计算时间= % s;目标成本= % s;ExitFlag = % s;迭代= % s \ n ',…num2str (t) num2str (info.Cost) num2str (info.ExitFlag) num2str (info.Iterations));

自动停车规划运行……计算时间= 12.3643;目标成本= 251.8455;ExitFlag = 1;迭代= 46

生成两个情节。一是停车过程的动画,蓝色的圆圈表示的最优路径和初始猜测是显示为一个点。其他显示最优轨迹的植物状态和控制动作。

TruckTrailerPlot (initialPose targetPose、参数信息,XY0);

下面的屏幕截图显示找到的最优轨迹MPC为不同的初始位置。

你可以尝试其他初始的x - y位置自动停车问题部分通过改变两个参数initialPose,只要是有效的位置。

如果ExitFlag是负的,非线性MPC控制器未能找到一个最优解,你不能信任返回的轨迹。在这种情况下,您可能需要提供更好的初始猜测并指定它simdata.InitialGuess在调用之前nlmpcmove。