主要内容

使用自适应模型预测控制避障

这个例子使用了:

打开脚本

本例展示了如何使用自适应MPC使车辆(自我车)遵循参考速度并避开车道上的障碍物。为此，您需要在运行时更新工厂模型和线性混合输入/输出约束。

避障

具有避障(或通过辅助)功能的车辆有一个传感器，如激光雷达，可以测量车辆前方和同一车道上障碍物的距离。障碍物可以是静态的，比如一个大坑，也可以是移动的，比如一辆缓慢移动的车辆。司机最常见的动作是暂时移到另一条车道，开过障碍物，然后移回原来的车道。

作为自动驾驶体验的一部分，避障系统可以在没有人为干预的情况下进行机动。在本例中，您设计了一个避障系统，该系统利用油门和转向角度使汽车绕过车道上的静态障碍物。该系统采用自适应模型预测控制器，在每个控制区间更新预测模型和混合输入/输出约束。

汽车模型

小轿车的外形为长5米、宽2米的矩形。该模型有四个状态:

  • x美元-汽车中心的全局X位置

  • y美元-汽车中心全局Y位置

  • \θ美元-车辆的航向角(0向东时，逆时针正)

  • 五美元-车速(正数)

有两个被操纵的变量:

  • 元新台币-油门(加速时为正，减速时为负)

  • \三角洲美元-转向角度(与汽车对齐时为0，逆时针正)

用一个简单的非线性模型来描述自我车的动力学:

数组$ $ \开始{}{1}& # xA; \点x = \因为左θ(\ \)\ \ cdot v \ \ & # xA;罪\·y = \ \离开θ(\ \)\ cdot v \ \ & # xA; \点\θ= \离开({{{\ tan \离开三角洲(\ \)}\ mathord{\左/ & # xA;{\ vphantom {{\ tan \离开三角洲(\ \)}{{C_L}}}} \对强生的# xA;\ kern - \ nulldelimiterspace} {{C_L}}}} \) \ cdot v \ \ & # xA; \•v = 0.5 \ cdot t # xA; \{数组}$ $

利用非线性状态空间模型的解析雅可比矩阵，建立了在标称工作点处的线性预测模型。

$$\begin{array}{l} \dot x = - v\sin \left(\theta \right) \cdot \theta + \cos \left(\theta \right) \cdot v\\ \dot y = v\cos \left(\theta \right) \cdot + \sin \left(\theta \right) \cdot v\\ {\ vphantom {{\ tan \离开三角洲(\ \)}{{C_L}}}} \对强生的# xA;\ kern - \ nulldelimiterspace} {{C_L}}}} \) \ cdot v + \离开({{{v \离开({\ tan{{\离开三角洲(\ \)}}^ 2 + 1}\右)}\ mathord{\左/ & # xA;{\ vphantom {{v \离开({\ tan{{\离开三角洲(\ \)}}^ 2 + 1}\右)}{{C_L}}}} \对强生的# xA;\ kern - \ nulldelimiterspace} {{C_L}}}} \) \ cdot \三角洲\ \ & # xA; \•v = 0.5 \ cdot t # xA; \{数组}$ $

在哪里C_L美元是车长。

假设所有的状态都是可测量的。在标称工作点，小轿车以恒定的速度向东行驶20.米每秒。

V = 20;X0 = [0;0;0;V];U0 = [0;0);

采用零阶保持器方法对连续时间模型进行离散化obstacleVehicleModelDT函数。

Ts = 0.02;[Ad,Bd,Cd,Dd,U,Y,X,DX] = obstacleVehicleModelDT(Ts,x0,u0);dsys = ss(Ad,Bd,Cd,Dd，“t”、Ts);dsys。InputName = {“节流”，“δ”};dsys。StateName = {“X”，“Y”，“θ”，“V”};dsys。OutputName = dsys.StateName;

道路和障碍信息

在本例中，假设:

  • 这条路笔直，有三条车道。

  • 每条车道宽4米。

  • 小轿车在不超车的时候开在中间车道的中间。

  • 在不失去一般性的情况下，自我车只从左边(快速)车道通过障碍。

车道= 3;laneWidth = 4;

在这个例子中的障碍是一个在中心车道中间的不移动的物体，与自我汽车大小相同。

障碍物=结构;障碍。长度= 5;障碍。宽度= 2;

放置障碍50几米远的路。

障碍。X = 50;障碍。Y = 0;

在障碍物周围创建一个虚拟的安全区，这样自我车在通过障碍物时就不会太靠近它。安全区域以障碍物为中心，具有:

  • 长度等于两辆车的长度

  • 宽度等于两个车道宽度

障碍。safeDistanceX = barrier . length;障碍。safeDistanceY = laneWidth;障碍物= obstacleGenerateObstacleGeometryInfo(障碍物);

在本例中，假设激光雷达设备可以检测到障碍物30.在车辆前方几米。

障碍。DetectionDistance = 30;

在标称条件下绘制如下图:

  • 自我车-绿色点与黑色边界

  • 水平车道-蓝色虚线

  • 障碍-红色x黑色边界

  • 安全区域-红色虚线边界。

f = obstaclePlotInitialCondition(x0，障碍物，车道宽度，车道);

标准工作点的MPC设计

设计一种模型预测控制器，使小车保持期望的速度并保持在中心车道的中间。

状态= mpcverbosity(“关闭”）;Mpcobj = mpc(dsys);

预测范围是25步数，相当于0.5秒。

mpcobj。PredictionHorizon = 60;% 25;mpcobj。ControlHorizon = 2;% 5;

为了防止小轿车加速或减速过快，在油门变化率上增加0.2 (m/s^2)的硬约束。

mpcobj.ManipulatedVariables(1)。RateMin = -0.2*Ts;mpcobj.ManipulatedVariables(1)。RateMax = 0.2*Ts;

类似地，在转向角变化率上添加每秒6度的硬约束。

mpcobj.ManipulatedVariables(2)。RateMin = -pi/30*Ts;mpcobj.ManipulatedVariables(2)。RateMax = pi/30*Ts;

按各自的操作范围缩放油门和转向角度。

mpcobj.ManipulatedVariables(1)。ScaleFactor = 2;mpcobj.ManipulatedVariables(2)。ScaleFactor = 0.2;

由于只有两个操纵变量，为了实现零稳态偏移，您可以只选择两个输出进行完美跟踪。在本例中，通过将其他两个输出(X和)的权重设置为零来选择Y位置和速度。这样做可以让这些其他输出的值浮动。

mpcobj.Weights.OutputVariables = [0 30 0 1];

更新控制器的正常工作状态。对于离散时间的植物:

  • U = u0

  • X = x0

  • Y = Cd*x0 + Dd*u0

  • DX = Ad*X0 + Bd*u0 - X0

mpcobj.Model.Nominal = struct(“U”U“Y”, Y,“X”, X,“DX”, DX);

为避障机动指定混合I/O约束

有不同的策略，使自我汽车避开道路上的障碍。例如，实时路径规划器可以在检测到障碍物后计算出一条新的路径，控制器会沿着这条路径前进。

在本例中，使用一种不同的方法，利用MPC显式处理约束的能力。当检测到障碍物时，它定义了道路上的一个区域(根据约束条件)，在预测范围内，自我车不能进入。在下一个控制间隔，根据自我车和障碍物的新位置重新定义区域，直到通过。

要定义要避免的区域，使用以下混合输入/输出约束:

E*u + F*y <= G

在哪里u被操纵的变量是向量和吗y是输出变量向量。您可以更新约束矩阵E，F,G当控制器运行时。

第一个约束是上的上界y美元（$y \le 6$在这条三车道的路上)。

E1 = [0 0];F1 = [0 1 0 0];G1 = laneWidth*lanes/2;

第二个约束是上的下界y美元（$y \ge -6$在这条三车道的路上)。

E2 = [0 0];F2 = [0 -1 0 0];G2 = laneWidth*lanes/2;

第三个约束条件是避障。即使在标称操作条件下没有检测到障碍，您也必须在这里添加一个“假”约束，因为您不能在运行时更改约束矩阵的维度。对于假约束，使用与第二个约束形式相同的约束。

E3 = [0 0];F3 = [0 -1 0 0];G3 = laneWidth*lanes/2;

控件在控制器中指定混合输入/输出约束setconstraint函数。

setconstraint (mpcobj [E1, E2、E3], [F1, F2, F3], [G1、G2、G3], [1; 1; 0.1]);

模拟控制器

在本例中，您使用了自适应MPC控制器，因为它比传统MPC控制器更有效地处理非线性车辆动力学。传统的MPC控制器采用恒定对象模型。然而，自适应MPC允许您在每个控制区间提供一个新的植物模型。由于新模型能更准确地描述新工况下的设备动态，自适应MPC控制器的性能优于传统MPC控制器。

此外，为了使控制器能够避开障碍物周围的安全区域，您需要在每个控制间隔更新第三个混合约束。基本上，自我车必须高于从自我车到安全区左上角形成的线。欲知详情，请打开obstacleComputeCustomConstraint．

使用常数参考信号。

refSignal = [0 0 0 V];

初始化设备和控制器状态。

X = x0;U = u0;egoStates = mpcstate(mpcobj);

模拟时间为4秒。

T = 0:Ts:4;

测井模拟数据进行绘图。

saveSlope = 0(长度(T)，1);saveIntercept = 0(长度(T)，1);ympc = 0(长度(T)，大小(Cd,1));umpc = 0(长度(T)，大小(Bd,2));

运行模拟。

为k = 1:长度(T)获得区间|k|的新工厂模型和输出测量值。[Ad,Bd,Cd,Dd,U,Y,X,DX] = obstacleVehicleModelDT(Ts, X, U);测量值= Cd * x + Dd * u;Ympc (k，:) =测量';确定车辆是否看到障碍物，并更新混合。% I/O约束时检测到障碍。检测= obstacleDetect(x，障碍物，laneWidth);[E, F, G, saveSlope (k), saveIntercept (k)] =.．.obstacleComputeCustomConstraint (x,检测、障碍、巷宽车道);为自适应MPC准备新的植物模型和标称条件。newPlant = ss(Ad,Bd,Cd,Dd，“t”、Ts);newNominal = struct(“U”U“Y”, Y,“X”, X,“DX”, DX);准备新的混合I/O约束。选项= mpcmoveopt;选项。CustomConstraint = struct(“E”, E,“F”F‘G’G);使用更新的植物，名义条件，计算最佳移动%和约束。[u,Info] = mpcmoveAdaptive(mpcobj,egoStates,newPlant,newNominal，.．.测量,refSignal,[],选项);Umpc (k，:) = u';为下一次迭代|k+1|更新工厂状态。x = Ad * x + Bd * u;结束mpcverbosity(状态);

分析结果

在避障机动过程中，绘制自我车的轨迹(黑线)和第三个混合I/O约束(虚线绿色)。

图(f)为k = 1: (saveSlope) X = [0;50;100];Y = saveSlope(k)*X + saveIntercept(k);线(X, Y,“线型”，“——”，“颜色”，‘g’）结束情节(ympc (: 1) ympc (:, 2),“- k”）;axis([0 ympc(end,1) -laneWidth*lanes/2 laneWidth*lanes/2])%复位轴

MPC控制器在没有人工干预的情况下成功完成了任务。

在Simulink中模拟控制器金宝app

打开Simulin金宝appk模型。避障系统包含多个组成部分:

  • 植物模型生成器:生成新的植物模型和标称值。

  • 障碍物检测器:检测障碍物(不包括激光雷达传感器)。

  • 约束生成器:产生新的混合I/O约束。

  • 自适应MPC:控制避障机动。

mdl =“mpc_ObstacleAvoidance”；open_system (mdl) sim (mdl)

模拟结果与命令行结果相同。为了支金宝app持快速原型工作流，您可以为避障系统中的块生成C/ c++代码。

bdclose (mdl)

另请参阅

块

功能

相关的话题