基于非线性模型预测控制的代客泊车服务

这个示例使用:

该示例演示了如何使用非线性模型预测控制(NLMPC)生成参考轨迹并跟踪泊车员的轨迹。

停车库

在这个例子中，停车车库包含一个自我车辆和八个静态障碍物。障碍物由六辆停放的车辆，保留停车区和车库边界给出。自我车辆的目标是停放在目标姿势，而不会与任何障碍碰撞。自我姿势的参考点位于后轴的中心。

定义自我车辆的参数。

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

指定初始自我载体姿态。

自我初始姿态:x(m)， y(m)，偏航角(rad)egoInitialpose = [4,12,0];

定义自我飞行器的目标姿势。在这个例子中，有两个可能的停车方向。朝北停车，设置parkNorth到真正的．到南方的公园，套装parkNorth到假．

parkNorth = true;如果parkNorth egoTargetPose = [3,45,pi/2];其他的egotargetospose = [27.2,4.7，-PI / 2];结束

这helperSLCreateCostmap函数创建停车场的静态地图，其中包含关于静止障碍物、道路标记和停放的车辆的信息。有关详细信息，请参见Simulink中的自动泊车服务金宝app（自动驾驶工具箱）的例子。

costmap = helperSLCreateCostmap ();centerToFront = distToCenter;centerToRear = distToCenter;helperSLCreateUtilityBus;costmapStruct = helperSLCreateUtilityStruct (costmap);

想象一下停车的环境。使用的样本时间0．1对于Visualizer。

电视= 0.1;helperSLVisualizeParkingValet (egoInitialPose 0 costmapStruct);

图自动泊车代客包含一个轴。标题为Parking garage的轴包含图像、矩形、直线、补丁、多边形等21个对象。

六辆停放的车辆是图中顶和底部的橙色盒子。中间区域代表保留停车区。车库的左边界也被建模为静态障碍。蓝色的自助式车辆有两个车轴和四个轮子。这两个绿色盒子代表自我车辆的目标停车位，顶部位置朝北。

使用非线性模型预测控制器生成轨迹

在这个例子中，运动学自行车模型与前转向角度是使用的。自我载体的运动可以用下面的方程来描述。

$\begin{array}{l} X_{}^{˙} = V. \cdot 因为（ ψ ） \\ y_{}^{˙} = V. \cdot 罪（ ψ ） \\ ψ_{}^{˙} = \frac{V.}{B.} \cdot 棕褐色（ δ ） \end{array}$

在哪里 $（ X 那 y ）$ 表示车辆的位置和 $ψ$ 表示飞行器的偏航角。的参数 $B.$ 代表车辆的轴距。 $（ X 那 y 那 ψ ）$ 为车辆状态函数的状态变量。的速度 $V.$ 和操舵角 $δ$ 是车辆状态功能的控制变量。

NLMPC控制器的停车位轨迹是根据类似于的分析设计的基于非线性模型预测控制的平行泊车的例子。实现了控制器的设计createMPCForParkingValet脚本。

自我车辆的速度被约束为在[-6.5,6.5] m / s内（大约用速度限制为15mph），并且自我载体的转向角受到约束的约束下降到-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(机器人系统工具箱)物体和从自助式车辆到障碍的距离是由checkCollision(机器人系统工具箱)功能。

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

解路径的初始猜想是由两条直线给出的。第一条线是从最初的自我载体姿态到一个中点，第二条线是从中点到自我载体目标姿态。

为初始解路径猜测选择一个中点。

如果parkNorth midPoint = [4,34,pi/2];其他的中点= [27,12,0];结束

配置NLMPC控制器参数。要在整个预测视界内规划最优轨迹，将控制视界设置为与预测视界相等。

%样品时间ts = 0.1;%预测地平线p = 100;%控制地平线c = 100;终端成本％重量矩阵Qt = 0.5 *诊断（[10 5 20]）;RT = 0.1 * DIAG（[1 2]）;%跟踪成本权重矩阵如果parkNorth Qp = 1e-6*diag([2 2 0]);Rp = 1e-4*diag([1 15]);其他的QP = 0 *诊断（[2 2 0]）;rp = 1e-2 * diag（[1 5]）;结束%到障碍物安全距离(m)SafetyDistance = 0.1;％最大迭代号码maxiter = 70;％禁用消息显示mpcverbosity (“关闭”）;

使用指定的参数创建NLMPC控制器。

[nlobj，opt，paras] = creatempcorparkingvalet（p，c，ts，egiinitialpose，egotargetpose，......麦克斯特,Qp, Rp, Qt, Rt distToCenter, safetyDistance,中点);

设定自我载体的初始条件。

x0 = egoInitialPose ';情况= (0,0);

生成参考轨迹使用nlmpcmove.功能。

tic;[MV，NLOPTIONS，INFO] = NLMPCMOVE（NLOBJ，X0，U0，[]，[]，选择）;timeval = toc;

得到状态的参考轨迹(xRef）和控制行动（uRef），这是计算预测地平线的最佳轨迹。

xRef = info.Xopt;uRef = info.MVopt;

分析计划的轨迹。

analyzeParkingValetResults (nlobj信息,egoTargetPose Qp, Rp, Qt, Rt,......distToCenter、safetyDistance timeVal)

结果摘要：1）有效结果。没有碰撞。2）到障碍的最小距离= 0.1297（当大于安全距离0.1000时有效）3）优化出口标志= 1（当正面时）4）NLMPCMOVE = 124.7698 5）的经过时间X（m）误差，Y（m）和θ（deg）：-0.0010,0011，-0.0772 6）最终控制输入速度（m / s）和转向角（deg）：-0.0174，-0.6954

如以下地图所示，计划的轨迹成功将自我车辆置于目标姿势中。最终控制输入值接近零。

plotTrajectoryParkingValet (xRef uRef)

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

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

在Simulink模型中跟踪参考轨迹金宝app

设计一个NLMPC控制器来跟踪参考轨迹。

首先，设置仿真持续时间，并根据持续时间更新参考轨迹。

持续时间= 12;tsteps =持续时间/ ts;XREF = [XREF（2：P + 1，:); REPMAT（XREF（exf（结束，:)，tsteps-p，1）];

使用跟踪预测地平线创建NLMPC控制器（Ptracking.)10.．

pTracking = 10;nlobjTracking = createMPCForTrackingParkingValet (pTracking Xref);

打开Simulin金宝appk模型。

mdl =“mpcAutoParkingValet”；open_system (mdl)

在运行模拟之前关闭动画图。

f = findobj ('姓名'那“自动停车员”）;关闭(f)

模拟模型。

SIM（MDL）

图自动泊车代客包含一个轴。标题为Parking garage的轴包含141个类型为图像、矩形、直线、补丁、多边形的对象。

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

动画显示，自我车辆成功地停在目标姿态没有任何障碍碰撞。您也可以使用ego vehicle pose和Controls范围查看ego vehicle和pose轨迹。

结论

此示例显示了如何使用非线性模型预测控制来生成参考轨迹并跟踪用于停车码的轨迹。控制器导航到目标停车处的自我车辆，而不会与任何障碍碰撞。

mpcverbosity ('在'）;if (mdl) = 1, if (mdl)'姓名'那“自动停车员”）;关闭(f)

基于非线性模型预测控制的代客泊车服务

停车库

使用非线性模型预测控制器生成轨迹

在Simulink模型中跟踪参考轨迹金宝app

结论

也可以看看

职能

块

相关的话题

模型预测控制工具箱文档

金宝app

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