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

这个例子使用了:

打开实时脚本

本例展示了如何利用非线性模型预测控制(NLMPC)设计并行泊车控制器。

停车环境

在本例中，停车环境包含一辆自我车辆和六个静态障碍物。这些障碍包括四辆停放的车辆、路边的道路和道路上的黄线。自我车辆的目标是在不与任何障碍物碰撞的情况下停在目标位置。车辆姿态的参考点位于后轴的中心。

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

vdims =车辆尺寸;egoWheelbase = vdim .轴距;distToCenter = 0.5*egoWheelbase;

自我载具以以下初始姿势开始。

X位置7米
Y位置3．1米
偏航角0rad

egoInitialPose = [7,3.1,0];

将自我车辆的中心停在目标位置(X =0， y =0)使用以下目标姿态，它指定了后轴参考点的位置。

X位置等于负X方向轴距长度的一半
Y位置0米
偏航角0rad

egoTargetPose = [-distToCenter,0,0];

想象停车环境。指定可视化器采样时间为0．1年代。

Tv = 0.1;helperSLVisualizeParking (egoInitialPose 0);

图自动平行泊车包含一个轴对象。标题为Parallel Parking的axis对象包含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到2米/秒之间，自我车辆的转向角度被限制在-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}$

为了提高仿真效率，NLMPC控制器提供了状态函数、代价函数和不等式约束的雅可比矩阵。不等式约束的雅可比矩阵由[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;Ny = 3;Nu = 2;Nlobj = nlmpc(nx,ny,nu);

指定采样时间(Ts)，预测视界(PredictionHorizon)，并控制视界(ControlHorizon)为控制器。

nlobj。Ts＝Ts；nlobj。PredictionHorizon = p;nlobj。ControlHorizon = c;

为被操作的变量定义约束。在这里,MV (1)自我车辆的速度是米/秒，和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 =“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 . constraintttolerance = 0.01;nlobj.Optimization.SolverOptions.OptimalityTolerance = 0.01;nlobj.Optimization.SolverOptions.MaxIter = maxIter;

定义最优状态解的初始猜测。这个初始猜想是从起始姿势到目标姿势的直线。方法中指定自我车辆参数的值nlmpcmoveopt对象。

选择= nlmpcmoveopt;opt.X0 = [linspace(egoInitialPose(1)，egoTargetPose(1)，p)'，.．.egoInitialPose linspace (egoInitialPose (2), (2), p)”.．.0 (p, 1)];opt.MV0 = 0 (p,nu);

计算代价函数和不等式约束，以及它们的雅可比矩阵，需要将参数传递给自定义函数。定义参数向量并指定参数的个数。中的参数值nlmpcmoveopt对象。

paras = {egoTargetPose,Qp,Rp,Qt,Rt,distToCenter,safetyDistance}';nlobj.Model.NumberOfParameters =数字(paras);opt.Parameters = paras;

MATLAB仿真控制器

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

控件模拟控制器nlmpcmove函数。
控件为控制器构建一个MEX文件buildMEX函数。对该MEX文件进行评估可以提高仿真效率nlmpcmove。

模拟NLMPC控制器停车使用runParkingAndPlot脚本。对于此模拟，不构建MEX文件(setuseMEX来0）.

useMex = 0;runParkingAndPlot

图自动平行泊车包含一个轴对象。标题为Parallel Parking的axis对象包含91个矩形、直线、补丁、多边形类型的对象。

图中包含3个轴对象。Axes对象1包含一个line类型的对象。坐标轴对象2包含一个line类型的对象。Axes对象3包含一个line类型的对象。

图中包含2个轴对象。Axes对象1包含一个stair类型的对象。坐标轴对象2包含一个楼梯类型的对象。

结果总结:1)有效结果。没有冲突。2)到障碍物的最小距离= 0.1775(当大于安全距离0.1000时有效)3)优化出口标志= 1(当为正时成功)4)nlmpcmove的耗时(s) = 13.0080 5) x (m)， y (m)和theta (deg)的最终状态误差:-0.0159,0.0290,0.1720 6)最终控制输入速度(m/s)和转向角度(deg): 0.0248， -1.2120

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

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

useMex = 1;runParkingAndPlot

从非线性MPC生成MEX函数parkingMex，加速仿真。代码生成成功。MEX函数parkingMex生成成功。

图自动平行泊车包含一个轴对象。标题为Parallel Parking的axis对象包含162个矩形、直线、补丁和多边形类型的对象。

图中包含3个轴对象。Axes对象1包含一个line类型的对象。坐标轴对象2包含一个line类型的对象。Axes对象3包含一个line类型的对象。

图中包含2个轴对象。Axes对象1包含一个stair类型的对象。坐标轴对象2包含一个楼梯类型的对象。

结果总结:1)无效结果。碰撞。2)到障碍物的最小距离= 0.0920(当大于安全距离0.1000时有效)3)优化出口标志= 2(当为正时成功)4)nlmpcmove的耗时(s) = 2.2652 5) x (m)， y (m)和theta (deg)的最终状态误差:-0.0503,0.0157,0.2315 6)最终控制输入速度(m/s)和转向角度(deg): 0.0719， -7.7646

使用MEX文件的模拟产生类似的结果，并且比使用MEX文件的模拟速度快得多nlmpcmove。

在Simulink中模拟控制器金宝app

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

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

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

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

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

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

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

模拟模型。

sim (mdl)

图自动平行泊车包含一个轴对象。标题为Parallel Parking的axis对象包含90个矩形、直线、补丁和多边形类型的对象。

ans = 金宝appSimulink。SimulationOutput: tout: [2533x1 double] SimulationMetadata: [1x1 金宝appSimulink. txt]SimulationMetadata] ErrorMessage: [0x0 char]

检查自我飞行器姿态和控制范围。

open_system ([mdl“/自我车辆模型/自我车辆姿势”(mdl) open_system (/控制的])

仿真结果与MATLAB仿真相似。自我车辆已经成功地停在目标位置，没有与任何障碍物碰撞。

结论

这个例子展示了如何设计一个并行泊车的非线性MPC控制器。控制器导航自我车辆到目标停车位而不与任何障碍物碰撞。

%启用消息显示mpcverbosity (“上”）;关闭Simuli金宝appnk模型bdclose (mdl)%关闭动画图F = findobj(“名字”，“自动平行泊车”）;关闭(f)

参考文献

[1]舒尔曼、约翰、段岩、何jonathan、亚历克斯·李、易卜拉欣·奥瓦尔、亨利·布拉德洛、潘佳、萨钦·帕蒂尔、肯·戈德堡和彼得·阿贝尔。“运动规划与连续凸优化和凸碰撞检查”。国际机器人研究杂志33岁的没有。9(2014年8月):1251-70。https://doi.org/10.1177/0278364914528132。