泊车员非线性模型预测控制

这个例子使用了:

打开实时脚本

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

停车场

在这个例子中，停车场包含一辆自我车辆和八个静态障碍物。障碍物由六辆停放的车辆、一个预留的停车场和车库边界组成。自我车辆的目标是在不与任何障碍物碰撞的情况下停在目标位置。自我姿态的参考点位于后轴的中心。

定义自我飞行器的参数。

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

指定初始自我车辆姿态。

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

定义自我飞行器的目标姿势。在这个例子中，有两个可能的停车方向。朝北停车，开始parkNorth来真正的．朝南停车，就位parkNorth来假．

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

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

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

想象停车环境。使用样本时间的0．1对于可视化工具。

Tv = 0.1;helperSLVisualizeParkingValet(egoInitialPose, 0, costmapStruct);

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

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

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

在本例中，采用了带前转向角的自行车运动模型。自我载具的运动可以用下列方程式来描述。

$\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]米/秒(大约限速为15英里/小时)，自我车辆的转向角度被限制在[-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}$

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

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

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

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

%采样时间Ts = 0.1;%预测水平P = 100;%控制水平C = 100;终端成本的权重矩阵Qt = 0.5*diag([10 5 20]);Rt = 0.1*diag([1 2]);用于跟踪成本的权重矩阵如果parkNorth Qp = 1e-6*diag([2 20 0]);Rp = 1e-4*diag([1 15]);其他的Qp = 0*diag([2 2 0]);Rp = 1e-2*diag([1 5]);结束%障碍物安全距离(m)safetyDistance = 0.1;最大迭代次数maxIter = 70;禁用消息显示mpcverbosity (“关闭”）;

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

[nlobj,opt,paras] = createMPCForParkingValet(p,c,Ts,egoInitialPose,egoTargetPose，.．.麦克斯特,Qp, Rp, Qt, Rt distToCenter, safetyDistance,中点);

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

x0 = egoInitialPose';U0 = [0;0];

方法生成参考轨迹nlmpcmove函数。

抽搐;(mv, nloptions信息)= nlmpcmove (x0, nlobj情况,[],[],选择);timeVal = toc;

得到状态的参考轨迹(xRef)和控制动作(uRef)，即预测视界计算出的最优轨迹。

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

分析计划轨迹。

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

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

如下图所示，规划的轨迹成功地将自我飞行器停在目标姿态。最终控制输入值接近于零。

plotTrajectoryParkingValet (xRef uRef)

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

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

Simulink模型中的轨迹参考轨迹金宝app

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

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

持续时间= 12;步骤=持续时间/Ts;Xref = [Xref (2: p + 1,:); repmat (Xref(最终,:),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: [127x1 double] SimulationMetadata: [1x1 金宝appSimulink. txt]SimulationMetadata] ErrorMessage: [0x0 char]

动画显示，自我车辆成功地停在目标姿势，没有任何障碍碰撞。你也可以使用自我飞行器姿态和控制范围来查看自我飞行器和姿态轨迹。

结论

本例展示了如何使用非线性模型预测控制生成参考轨迹并跟踪代客泊车轨迹。控制器导航自我车辆到目标停车位而不与任何障碍物碰撞。

mpcverbosity (“上”）;Bdclose (mdl) f = findobj(“名字”，“自动泊车代客”）;关闭(f)

泊车员非线性模型预测控制

停车场

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

Simulink模型中的轨迹参考轨迹金宝app

结论

另请参阅

功能

块

相关的话题

模型预测控制工具箱文档

金宝app

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