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

此示例使用：

打开直播脚本

这个例子展示了如何使用非线性模型预测控制(NLMPC)设计并联泊车控制器。

停车环境

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

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

vdims = vehicleDimensions;egoWheelbase = vdims.Wheelbase;distToCenter = 0.5 * egoWheelbase;

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

x位置7米
y位置3.1米
偏航角0rad

egoInitialPose = [7, 3.1, 0);

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

x位等于负x方向的轴距长的一半
y位置0米
偏航角0rad

EgotargetPose = [-DistTocenter，0,0];

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

电视= 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$ 和转向角度 $δ.$ 为车辆状态函数的控制变量。实现了车辆状态函数parkingVehicleStateFcn．

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

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

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

$J ＝ \int_{0}^{d} （年代（ t ） - {年代}_{r e f} ）^{T} 问_{p} （年代（ t ） - {年代}_{r e f} ） + u （ t ）^{T} R_{p} u （ t ） d t + （年代（ d ） - {年代}_{r e f} ）^{T} 问_{t} （年代（ d ） - {年代}_{r e f} ） + u （ d ）^{T} R_{t} u （ d ）$

为避免与障碍物的碰撞，NLMPC控制器必须满足以下不等式约束，其中与所有障碍物的最小距离 $d 我年代 t_{米我 n}$ 一定要大于安全距离吗 $d 我年代 t_{年代一个 f e}$ ．在这个例子中，自我车辆和障碍物被建模为collisionbox.（机器人系统工具箱）目标和自我车辆到障碍的距离计算使用核对机构（机器人系统工具箱）．

$d 我年代 t_{米我 n} \geq d 我年代 t_{年代一个 f e}$

为提高仿真效率，给出了状态函数、代价函数和不等式约束的雅可比矩阵。在[1]的基础上逼近了不等式约束的雅可比矩阵。
状态解的初始猜测由自我载体的初始和目标姿态之间的直线定义。金宝搏官方网站

指定样本时间(TS.），预测地平线（p）和控制地平线（米）对于非线性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;

指定控制器状态函数和状态函数雅可比矩阵。

nlobj.model.statefcn =.“停车有奇特尔斯特菲尔特”；nlobj.jacobian.statefcn =.“parkingVehicleStateJacobianFcn”；

指定控制器成本函数和成本函数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）'．．.0 (p, 1)];opt.MV0 = 0 (p,ν);

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

帕拉斯= {egoTargetPose Qp, Rp, Qt, Rt, distToCenter, safetyDistance} ';nlobj.Model.NumberOfParameters =元素个数(帕拉斯);opt.Parameters =帕拉斯;

模拟MATLAB中的控制器

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

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

仿真NLMPC控制器的停车使用runParkingAndPlot脚本。对于这个模拟，不要构建MEX文件(setUSEMEX.来0）。

useMex = 0;runParkingAndPlot

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

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

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

结果总结:1)有效结果。没有冲突。2)最小距离障碍= 0.1813(有效当大于安全距离0.1000)3)优化出口标志= 1(成功当积极)4)运行时间(s) nlmpcmove = 32.4323 5)最终状态错误x (m), y (m)和θ(度):-0.0011,0.0267,0.1539 6)最终控制输入速度(米/秒)和转向角(度):0.0043,-0.1275

自我的车辆成功地停在目标位置。最终的控制输入值接近于零。在动画中，自我载体在任何时候都不会与任何障碍相撞。

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

USEMEX = 1;runParkingAndPlot

从非线性MPC生成MEX函数“parkingMex”以加速模拟。代码生成成功。MEX函数“parkingMex”成功生成。

自动平行停车包含一个轴。标题为“平行停车”的轴包含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中模拟控制器金宝app

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

指定模拟持续时间并打开Simulink模型。金宝app

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

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

CreateparameterBus（nlobj，[mdl'/非线性MPC控制器']，“parasBusObject”帕拉斯);

在模拟模型之前关闭动画情节。

f = findobj（“名字”，'自动并行停车'）;关闭（f）

模拟模型。

sim (mdl)

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

ans = 金宝appsimulink.simulationOutput：tout：[2605x1 double] simulation metaData：[1x1 simulink.simulation metaData] errormessage：[0x0 char]

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

Open_System（[MDL“自我车辆模型/自我车辆姿态”(mdl) open_system (/控制的]）

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

结论

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

％启用消息显示mpcverbosity（'在'）;％关闭Simul金宝appink模型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(8月2014):1251-70。https://doi.org/10.1177/0278364914528132．

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

停车环境

自我车辆模型

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

模拟MATLAB中的控制器

在Simulink中模拟控制器金宝app

结论

参考

也可以看看

功能

块

相关话题

模型预测控制工具箱文档

金宝app

使用Simulink实现Adaptive Cruise控制器金宝app