基于非线性模型预测控制的平行泊车

这个示例使用:

打开生活的脚本

这个例子展示了如何使用非线性模型预测控制(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);

想象一下停车的环境。指定可视化程序的示例时间0．1年代。

电视= 0.1;helperSLVisualizeParking (egoInitialPose 0);

自动平行泊车包含一个轴对象。标题为“平行停车”的轴对象包含20个类型为矩形、直线、补丁、多边形的对象。

在可视化中，四辆停着的车辆是中间的橙色盒子。下面的橙色边界是道路的边沿，上面的橙色边界是道路上的黄线。

自我的汽车模型

对于停车问题，车辆以低速行驶。这个例子使用一个带有前转向角度的运动学自行车模型来解决车辆停放问题。自我载体的运动可以用下面的方程来描述。

$\begin{array}{l} x_{}^{˙} ＝ v \cdot 因为（ ψ ） \\ y_{}^{˙} ＝ v \cdot 罪（ ψ ） \\ ψ_{}^{˙} ＝ \frac{v}{b} \cdot 棕褐色（ δ ） \end{array}$

在这里, $（ x ， y ）$ 表示车辆的位置和 $ψ$ 表示飞行器的偏航角。的参数 $b$ 表示车辆的轴距。 $（ x ， y ， ψ ）$ 是车辆状态函数的状态变量。的速度 $v$ 和操舵角 $δ$ 为车辆状态函数的控制变量。实现了车辆状态函数parkingVehicleStateFcn．

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

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

车辆状态函数的输出与车辆状态相同 $（ x ， y ， ψ ）$ ．因此，使用三个状态、三个输出和两个操作变量创建NLMPC控制器对象。
自我车辆的速度被限制在-2 - 2m /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(机器人系统工具箱)目标和自我车辆到障碍的距离计算使用checkCollision(机器人系统工具箱)．

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

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

指定样本时间(Ts)、预测视界(p)，以及控制层(米)的非线性MPC控制器。

t = 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求解器的最大迭代次数。

麦克斯特= 40;

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

mpcverbosity (“关闭”）;

创建nlmpc具有三个状态、三个输出和两个输入的控制器对象。

nx = 3;纽约= 3;ν= 2;nlobj = nlmpc (nx、纽约、ν);

指定样本时间(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 = -π/ 4;nlobj.MV(2)。Max =π/ 4;

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

nlobj.Model.StateFcn =“parkingVehicleStateFcn”；nlobj.Jacobian.StateFcn =“parkingVehicleStateJacobianFcn”；

指定控制器的代价函数和代价函数雅可比矩阵。

nlobj.Optimization.CustomCostFcn =“parkingCostFcn”；nlobj.Optimization.ReplaceStandardCost = true;nlobj.Jacobian.CustomCostFcn =“parkingCostJacobian”；

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

nlobj.Optimization.CustomIneqConFcn =“parkingIneqConFcn”；nlobj.Jacobian.CustomIneqConFcn =“parkingIneqConFcnJacobian”；

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

nlobj.Optimization.SolverOptions.FunctionTolerance = 0.01;nlobj.Optimization.SolverOptions.StepTolerance = 0.01;nlobj.Optimization.SolverOptions.ConstraintTolerance = 0.01;nlobj.Optimization.SolverOptions.OptimalityTolerance = 0.01;nlobj.Optimization.SolverOptions.MaxIter =麦克斯特;

定义最优状态解的初始猜想。这个初始猜想是从开始姿势到目标姿势的直线。属性中为ego车辆参数指定值nlmpcmoveopt对象。

选择= nlmpcmoveopt;opt.X0 = [linspace (egoInitialPose (1) egoTargetPose (1), p)”,．．.egoInitialPose linspace (egoInitialPose (2), (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个轴对象。axis对象1包含一个类型为line的对象。axis对象2包含一个类型为line的对象。axis对象3包含一个类型为line的对象。

图中包含2个轴对象。axis对象1包含一个楼梯类型的对象。axis对象2包含一个楼梯类型的对象。

结果总结:1)有效结果。没有冲突。2)最小距离障碍= 0.1813(有效当大于安全距离0.1000)3)优化出口标志= 1(成功当积极)4)运行时间(s) nlmpcmove = 37.8317 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个轴对象。axis对象1包含一个类型为line的对象。axis对象2包含一个类型为line的对象。axis对象3包含一个类型为line的对象。

图中包含2个轴对象。axis对象1包含一个楼梯类型的对象。axis对象2包含一个楼梯类型的对象。

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

使用MEX文件的模拟产生类似的结果，并且比使用nlmpcmove．

在Simulink中模拟控制器金宝app

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

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

时间= p * Ts;mdl =“mpcVDAutoParking”；open_system (mdl)

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

(mdl createParameterBus (nlobj“/非线性MPC控制器”]，“parasBusObject”帕拉斯);

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

f = findobj (“名字”，“自动平行泊车”）;关闭(f)

模拟模型。

sim (mdl)

自动平行泊车包含一个轴对象。标题为“平行停车”的轴对象包含90个类型为矩形、直线、补丁、多边形的对象。

ans =仿金宝app真软件。SimulationMetadata: [1x1 Simulink. out: out: [2615x1 double]金宝appSimulationMetadata]错误信息:[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 Abbeel。“基于顺序凸优化和凸碰撞检查的运动规划”。国际机器人研究杂志33岁的没有。9(8月2014):1251-70。https://doi.org/10.1177/0278364914528132．

基于非线性模型预测控制的平行泊车

停车环境

自我的汽车模型

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

MATLAB仿真控制器

在Simulink中模拟控制器金宝app

结论

参考文献

另请参阅

功能

块

相关的话题

模型预测控制工具箱文档

金宝app

用Simulink实现自适应巡航控制器金宝app