使用非线性模型预测控制并行停车

此示例使用：

打开直播脚本

此示例显示如何使用非线性模型预测控制（NLMPC）设计并行停车控制器。

停车环境

在这个例子中，停车环境包含自我车辆和六个静态障碍物。障碍包括四辆停放的车辆，路边路边，以及道路上的黄线。自我车辆的目标是停放在目标姿势，而不会与任何障碍碰撞。EGO车辆姿势的参考点位于后桥的中心。

自我车辆有两个车轴和四个轮子。定义自我车辆参数。

vdims =车辆施用;egowheelbase = vdims.wheelbase;Disttocenter = 0.5 * egowheelbase;

自我车辆从以下初始姿势开始。

x位置7.m
y位置3.1m
偏航角度0.Rad.

egoInitialpose = [7,3.1,0];

将自我车辆的中心停在目标位置（x =0.，y =0.）使用以下目标姿势，指定后轴参考点的位置。

x位等于负x方向的轴距长的一半
y位置0.m
偏航角度0.Rad.

EgotargetPose = [-DistTocenter，0,0];

可视化停车环境。指定Visualizer采样时间0.1s。

电视= 0.1;evergerslvisualizeparking（egoInitialpose，0）;

图自动化并行停车包含轴。具有标题并行停车的轴包含20个矩形，线，贴片，多边形的20个对象。

在可视化中，四辆停放的车辆是中间的橙色盒子。底部橙色边界是道路路边，顶部橙色边界是道路上的黄线。

自我车辆模型

对于停车问题，车辆以低速行进。此示例使用带有前转向角的运动自行车模型，用于车辆停车问题。可以使用以下等式描述自我车辆的运动。

$\begin{array}{l} X_{}^{˙} = V. \cdot COS. （ ψ ） \\ y_{}^{˙} = V. \cdot 罪（ ψ ） \\ ψ_{}^{˙} = \frac{V.}{B.} \cdot 晒黑（ δ. ） \end{array}$

这里， $（ X 那 y ）$ 表示车辆的位置和 $ψ$ 表示车辆的偏航角度。参数 $B.$ 代表车辆的轴距。 $（ X 那 y 那 ψ ）$ 是车辆状态功能的状态变量。速度 $V.$ 和转向角度 $δ.$ 是车辆状态功能的控制变量。车辆状态功能在中实现停车前克雷斯特省。

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

用于停车的非线性模型预测控制器基于以下分析设计。

车辆状态功能的输出与车辆的状态相同 $（ X 那 y 那 ψ ）$ 。因此，NLMPC控制器对象是用三个态，三个输出和两个被操纵变量创建的。
自我车辆的速度受到约束为-2和2 m / s，自工载体的转向角受到约束至-45至45度之间。
NLMPC控制器使用定制成本函数，其以类似于二次跟踪成本加上终端成本的方式定义。在以下定制成本函数中， $S. （ T. ）$ 表示自我车辆的状态 $T.$ 那 $D.$ 代表模拟的持续时间， ${S.}_{R. E. F}$ 是自我车辆的目标姿势。重量矩阵 ${问：}_{P.}$ 那 ${R.}_{P.}$ 那 ${问：}_{T.}$ ，和 ${R.}_{T.}$ 是恒定的。

$j = \int_{0.}^{D.} （ S. （ T. ） - {S.}_{R. E. F} ）^{T.} {问：}_{P.} （ S. （ T. ） - {S.}_{R. E. F} ） + 你（ T. ）^{T.} {R.}_{P.} 你（ T. ） D. T. + （ S. （ D. ） - {S.}_{R. E. F} ）^{T.} {问：}_{T.} （ S. （ D. ） - {S.}_{R. E. F} ） + 你（ D. ）^{T.} {R.}_{T.} 你（ D. ）$

为避免与障碍物的碰撞，NLMPC控制器必须满足以下不等式约束，其中与所有障碍物的最小距离 $D. 一世 S. {T.}_{m 一世 N}$ 必须大于安全距离 $D. 一世 S. {T.}_{S. 一种 F E.}$ 。在这个例子中，自我车辆和障碍物被建模为collisionbox.（机器人系统工具箱）计算物体和自我车辆到障碍物的距离核对机构（机器人系统工具箱）。

$D. 一世 S. {T.}_{m 一世 N} \geq D. 一世 S. {T.}_{S. 一种 F E.}$

为了提高模拟效率，状态功能的雅各比亚人，成本函数和不等式约束都提供给NLMPC控制器。不等式约束的雅各比亚人基于[1]近似。
状态解决方案的初始猜测由自由车辆的初始和目标姿势之间的直线限定。万博尤文图斯

指定采样时间（TS.），预测地平线（P.）和控制地平线（m）对于非线性MPC控制器。

ts = 0.1;p = 70;C = 70;

为控制器指定常量重量矩阵。定义跟踪权重矩阵（QP.和rp.）和终端重量矩阵（Qt.和RT.）。

QP = DIAG（[0.1 0.1 0]）;RP = 0.01 *眼睛（2）;qt = diag（[1 5 100]）;RT = 0.1 *眼睛（2）;

指定安全距离0.1m，控制器在定义其约束时使用该。

SafetyDistance = 0.1;

指定NLMPC求解器的最大迭代次数。

maxiter = 40;

创建非线性MPC控制器。为清楚起见，首先禁用MPC命令窗口消息。

mpcverbosity（'离开'）;

创造nlmpc.控制器对象具有三个状态，三个输出和两个输入。

nx = 3;纽约= 3;nu = 2;nlobj = nlmpc（nx，ny，nu）;

指定采样时间（TS.），预测地平线（predictionhorizon.）和控制地平线（ControlHorizon.）对于控制器。

nlobj.ts = ts;nlobj.predictionhorizon = p;nlobj.controlhorizon = c;

定义用于操纵变量的约束。这里，MV（1）是m / s的自我车速，MV（2）是弧度的转向角。

nlobj.mv（1）.min = -2;nlobj.mv（1）.max = 2;nlobj.mv（2）.min = -pi / 4;nlobj.mv（2）.max = pi / 4;

指定控制器状态函数和状态函数jacobian。

nlobj.model.statefcn =.“停车有奇特尔斯特菲尔特”;nlobj.jacobian.statefcn =.“停车有兴趣的jacobianfcn”;

指定控制器成本函数和成本函数jacobian。

nlobj.optimization.customcostfcn =.“停车COSTFCN”;nlobj.optimization.replacestandardcost = true;nlobj.jacobian.customcostfcn =.“停车Costjacobian”;

定义控制器和约束雅可比的自定义不等式约束。自定义约束函数计算将自我车辆形成到环境中的所有障碍物的距离，并将这些距离与安全距离进行比较。

nlobj.optimization.customineqconfcn =“停车场QCONFCN”;nlobj.jacobian.customineqconfcn =.“停车场Qconfcnacobian”;

配置控制器的优化求解器。

nlobj.optimization.solveroptions.functionstolerance = 0.01;nlobj.optimization.solveroptions.steptolerance = 0.01;nlobj.optimization.solveroptions.constrainttolerance = 0.01;nlobj.optimization.solveroptions.optimalanytolerance = 0.01;nlobj.optimization.solveroptions.maxiter = maxiter;

定义最佳状态解决方案的初始猜测。这个初步猜测是从起始姿势到目标姿势的直线。此外，指定自我车辆参数的值nlmpcmoveopt.目的。

opt = nlmpcmoveopt;opt.x0 = [linspace（egoInitialpose（1），egotargetospose（1），p）'，......Linspace（egoInitialpose（2），egoInitialpose（2），p）'......零（p，1）];opt.mv0 =零（p，nu）;

计算成本函数和不等式约束以及雅克索人，需要将参数传递给自定义函数。定义参数向量并指定参数的数量。此外，指定参数值nlmpcmoveopt.目的。

Paras = {EgotargetPosite，QP，RP，QT，RT，Distrocenter，SafetyDistance}';nlobj.model.numberofparameters = numel（paras）;opt.parameters = paras;

模拟MATLAB中的控制器

要在MATLAB®中模拟NLMPC控制器，您可以使用以下选项之一：

使用该控制器模拟控制器nlmpcmove.功能。
使用使用的控制器构建一个MEX文件buildmex.功能。评估此MEX文件可提高模拟效率nlmpcmove.。

模拟使用该控制器的NLMPC控制器runparkingandplot.脚本。对于此模拟，请勿构建MEX文件（设置USEMEX.到0.）。

USEMEX = 0;runparkingandplot.

图自动化并行停车包含轴。具有标题并联停车的轴包含91型矩形，线，贴片，多边形的物体。

图包含3个轴。轴1包含类型线的对象。轴2包含类型线的对象。轴3包含类型线的对象。

图包含2个轴。轴1包含楼梯类型的物体。轴2包含楼梯类型的物体。

结果摘要：1）有效结果。没有碰撞。2）与障碍物的最小距离= 0.1813（当大于安全距离0.1000时有效）3）优化出口标志= 1（当正面时）4）对于NLMPCMove = 32.4323 5）的经过时间= 32.4323 5）x（m）中的最终状态误差，Y（m）和θ（deg）：-0.0011,0.0267,0.1539 6）最终控制输入速度（m / s）和转向角（deg）：0.0043，-0.1275

EGO车辆在目标中成功姿势。最终控制输入值接近零。在动画中，当时车辆在任何时候都不碰撞任何障碍物。

为控制器构建MEX文件并重新运行模拟。

USEMEX = 1;runparkingandplot.

从非线性MPC生成MEX功能“SANDMEX”以加快仿真。代码成功。MEX函数“SAINDMEX”成功生成。

图自动化并行停车包含轴。具有标题并联停车的轴包含162个类型的矩形，线，贴片，多边形。

图包含3个轴。轴1包含类型线的对象。轴2包含类型线的对象。轴3包含类型线的对象。

图包含2个轴。轴1包含楼梯类型的物体。轴2包含楼梯类型的物体。

结果摘要：1）有效结果。没有碰撞。2）到障碍物的最小距离= 0.1798（当大于安全距离0.1000时有效）3）优化出口标志= 1（当正面时）4）对于NLMPCMove = 4.0807 5）X（m）的最终状态误差，Y（m）和θ（deg）：-0.0122,0.0273,0.1465 6）最终控制输入速度（m / s）和转向角（deg）：0.0068，0.4843

使用MEX文件的仿真产生类似的结果，并且比使用模拟速度更快nlmpcmove.。

在Simulink中模拟控制器万博1manbetx

要在Simulink®中模拟NLMPC控制器，请使用万博1manbetx非线性MPC控制器堵塞。对于这个例子，为了模拟自我车辆，使用横向块的车身3D，这是一个自行车模型（自动驾驶工具箱）堵塞。

指定模拟持续时间并打开Simulink模型。万博1manbetx

持续时间= p * ts;mdl ='mpcvdautoparking';Open_System（MDL）

要将EGO车辆参数传递给控制器，您必须创建一个参数总线对象。

CreateparameterBus（nlobj，[mdl'/非线性MPC控制器']，'parasbusobject'，帕拉斯）;

在模拟模型之前关闭动画绘图。

f = findobj（'名称'那'自动并行停车'）;关闭（f）

模拟模型。

SIM（MDL）

图自动化并行停车包含轴。具有标题并联停车的轴包含90个类型的矩形，线，贴片，多边形。

ans = 万博1manbetxsimulink.simulationOutput：tout：[2605x1 double] simulation metaData：[1x1 simulink.simulation metaData] errormessage：[0x0 char]

检查自我车辆姿势和控制范围。

Open_System（[MDL'/自我车型/自我车辆姿势']）Open_System（[MDL'/控制']）

仿真结果类似于MATLAB仿真。自我车辆已经成功停在目标姿势，没有任何障碍物。

结论

此示例显示了如何设计用于并行停车的非线性MPC控制器。控制器导航到目标停车处的自我车辆，而不会与任何障碍碰撞。

％启用消息显示mpcverbosity（'在'）;％关闭Simul万博1manbetxink模型BdClose（MDL）％关闭动画情节f = findobj（'名称'那'自动并行停车'）;关闭（f）

参考

[1] Schulman，John，Yan Duan，Jonathan Ho，Alex Lee，Ibrahim Awwal，Henry Bradlow，Jia Pan，Sachin Patil，Ken Goldberg和Pieter Bebeel。'具有顺序凸优化和凸碰撞检查的运动规划'。国际机器人研究杂志33，不。9（2014年8月）：1251-70。https://doi.org/10.1177/0278364914528132。

使用非线性模型预测控制并行停车

停车环境

自我车辆模型

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

模拟MATLAB中的控制器

在Simulink中模拟控制器万博1manbetx

结论

参考

也可以看看

功能

块

相关话题

模型预测控制工具箱文档

万博1manbetx

使用Simulink实现Adaptive Cruise控制器万博1manbetx