代客停车使用非线性模型预测控制

这个示例使用:

这个例子展示了如何生成一个参考轨迹和跟踪的轨迹代客停车使用非线性模型预测控制(NLMPC)。

停车场

包含一个自我在这个例子中,停车场车辆和八个静态障碍。障碍是由六个停放车辆,保留停车场,车库边境。自我的目标车辆停车目标姿势没有碰撞的障碍。自我的参照点姿势位于后桥的中心。

定义参数的自我。

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

指定初始自我车辆构成。

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

定义的目标构成自我。在这个例子中,有两个可能的方向停车。公园面对北,集parkNorth来真正的。朝南的公园,集parkNorth来假。

parkNorth = true;如果parkNorth egoTargetPose =(36岁,45岁,π/ 2);其他的egoTargetPose =(27.2, 4.7, -π/ 2);结束

的helperSLCreateCostmap函数创建一个静态的停车场地图包含静止的障碍物信息,路标,停放的汽车。更多细节,请参阅自动停车员在仿真软件金宝app(自动驾驶工具箱)的例子。

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

可视化的停车环境。使用一个样本的时间0.1可视化工具。

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

图自动停车员包含一个坐标轴对象。坐标轴对象与标题停车场包含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与限速(约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}$

解决方案的初始猜测路径是由两条直线。第一行是从最初的自我车辆对中间点,从中间,第二行是指向自我的车辆目标构成。

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

如果34岁的parkNorth中点=[4π/ 2);其他的中点=[0]27日12日;结束

配置NLMPC控制器的参数。在整个计划最优轨迹预测地平线,地平线设置控制等于预测地平线。

%样品时间t = 0.1;%预测地平线p = 100;%控制地平线c = 100;%重量矩阵终端成本Qt = 0.5 *诊断接头([10 5 20]);Rt = 0.1 *诊断接头([1 - 2]);%重量矩阵跟踪成本如果parkNorth Qp = 1 e-6 *诊断接头((2 2 0));Rp = 1 *的军医诊断接头(15 [1]);其他的Qp = 0 *诊断接头((2 2 0));Rp = 1依照*诊断接头([1 5]);结束%的安全距离障碍(m)safetyDistance = 0.1;%最大迭代次数麦克斯特= 70;%禁用消息显示mpcverbosity (“关闭”);

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

[nlobj,选择,帕拉斯]= createMPCForParkingValet (p、c、t、egoInitialPose egoTargetPose,…麦克斯特,Qp, Rp, Qt, Rt distToCenter, safetyDistance,中点);

设置初始条件的自我。

x0 = egoInitialPose ';情况= (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.1272(有效当大于安全距离0.1000)3)优化出口标志= 2(成功当积极)4)运行时间(s) nlmpcmove = 33.2296 5)最终状态错误x (m), y (m)和θ(度):-0.0004,0.0412,-1.9488 6)最终控制输入速度(米/秒)和转向角(度):-0.2190,4.5549

如以下所示情节,计划的成功轨迹公园自我车辆目标构成。最后控制输入值接近于零。

plotTrajectoryParkingValet (xRef uRef)

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

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

跟踪参考轨迹仿真软件模型金宝app

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

首先,设置仿真时间和更新基于持续时间的参考轨迹。

时间= 12;Tsteps = / Ts持续时间;Xref = [Xref (2: p + 1,:); repmat (Xref(最终,:),Tsteps-p, 1)];

创建一个NLMPC控制器跟踪预测地平线(pTracking)10。

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

打开仿真软件模型。金宝app

mdl =“mpcAutoParkingValet”;open_system (mdl)

在运行模拟前关闭动画情节。

f = findobj (“名字”,“自动停车员”);关闭(f)

模拟模型。

sim (mdl)

图自动停车员包含一个坐标轴对象。坐标轴对象与标题停车场包含141个对象类型的形象,矩形,线,补丁,多边形。

ans =仿金宝app真软件。SimulationOutput:吹捧:x1双[127]SimulationMetadata: [1 x1仿真软件。金宝appSimulationMetadata] ErrorMessage (0 x0 char):

动画显示,自我汽车公园在目标造成碰撞成功没有任何障碍。您还可以查看自我车辆和姿势使用自我车辆姿态和轨迹控制范围。

结论

这个例子展示了如何生成一个参考轨迹和跟踪轨迹的代客停车使用非线性模型预测控制。控制器自我导航车辆目标停车位没有碰撞与任何障碍。

mpcverbosity (“上”);bdclose (mdl) f = findobj (“名字”,“自动停车员”);关闭(f)

代客停车使用非线性模型预测控制

停车场

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

跟踪参考轨迹仿真软件模型金宝app

结论

另请参阅

功能

块

相关的话题