这个例子展示了如何使用自适应MPC使车辆(ego car)遵循参考速度并避开车道上的障碍物。为此,您需要在运行时更新工厂模型和线性混合输入/输出约束。
具有避障(或过路辅助)功能的车辆装有传感器,如激光雷达,可以测量车辆在同一车道上与前方障碍物的距离。障碍物可以是静态的,比如一个大的壶洞,也可以是移动的,比如一个缓慢移动的车辆。司机最常见的动作是暂时移到另一条车道,越过障碍物,然后回到原来的车道。
作为自动驾驶体验的一部分,避障系统可以在没有人工干预的情况下执行机动操作。在本例中,您将设计一个避障系统,使用油门和转向角度使ego汽车绕过车道上的静态障碍物。该系统采用自适应模型预测控制器,在每个控制区间更新预测模型和混合输入/输出约束。
ego车是矩形的,长5米,宽2米。模型有四种状态:
-汽车中心的全球X位置
- 汽车中心环球Y位置
- 汽车的航向角(0
朝东的时候,逆时针为正)
- 汽车的速度(正)
有两个调节变量:
- 油门(加速时,减速时,负得正)
-转向角度(与车身对准时为0,逆时针方向为正)
使用一个简单的非线性模型来描述自我汽车的动力学:
利用非线性状态空间模型的解析雅可比矩阵构造了标称工作点的线性预测模型。
哪里为汽车长度。
假设所有的状态都是可测量的。在名义操作点,自我汽车以恒定的速度向东行驶20
米每秒。
V = 20;x0 = [0;0;0;V];情况= [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;
在本例中,假设:
这条路是直的,有三条车道。
每个车道四米宽。
自负车在没有超车时,会在中间车道的中间行驶。
在不失去一般性的情况下,自我车只从左边(快速)车道通过障碍。
泳道= 3;laneWidth = 4;
在这个例子中的障碍就是与同样大小的汽车自我中间车道中间的不移动的物体。
障碍=结构;障碍。长度= 5;障碍。宽度= 2;
把障碍50
米的道路。
障碍。X = 50;障碍。Y = 0;
在障碍物周围创建一个虚拟安全区,这样自我车在通过障碍物时不会太靠近障碍物。安全区以障碍物为中心,并且有:
长度等于两辆车的长度
宽度为两米车道宽度
obstacle.safeDistanceX = obstacle.Length;obstacle.safeDistanceY = laneWidth;障碍物= obstacleGenerateObstacleGeometryInfo(障碍物);
在本例中,假设激光雷达设备能够探测到障碍物三十
在车辆前面几米远的地方。
obstacle.DetectionDistance = 30;
绘制在额定条件如下:
颐高车 - 绿点与黑边界
水平线-蓝色虚线
障碍 - 红X
用黑色边界
安全区-虚线红色边界。
f = obstaclePlotInitialCondition (x0,障碍、巷宽车道);
设计一个模型预测控制器,可以使自我汽车保持一个理想的速度,并停留在中心车道的中间。
状态= mpcverbosity (“关闭”);mpcobj = mpc (dsys);
预测范围是25
步长,相当于0。5秒。
mpcobj。PredictionHorizon = 60;%25;mpcobj.ControlHorizon = 2;% 5;
为了防止自负车加速或减速过快,在油门变化率上增加0.2 (m/s^2)的硬约束。
mpcobj.ManipulatedVariables (1)。杀鼠灵= -0.2 * Ts;mpcobj.ManipulatedVariables (1)。RateMax = 0.2 * Ts;
同样,在转向角变化率上增加每秒6度的硬约束。
mpcobj.ManipulatedVariables(2)。杀鼠灵= -π/ 30 * Ts;mpcobj.ManipulatedVariables(2)。RateMax =π/ 30 * Ts;
按各自的操作范围调整油门和转向角度。
mpcobj.ManipulatedVariables(1).ScaleFactor = 2;mpcobj.ManipulatedVariables(2).ScaleFactor = 0.2;
由于只有两个操纵变量,为了实现零稳态偏移,您可以只选择两个输出来实现完美跟踪。在这个例子中,通过设置其他两个输出(X和)的权重为零来选择Y的位置和速度。这样做可以让其他输出的值浮动。
mpcobj.Weights.OutputVariables = [0 30 0 1];
更新与标称工作状态的控制器。对于离散时间对象:
U =情况
X = X0
Y = Cd*x0 + Dd*u0
DX = Ad*X0 + Bd*u0 - X0
mpcobj.Model。名义=结构(“U”U“Y”,Y,“X”,X,'DX',DX);
有不同的策略让自我汽车避开道路上的障碍。例如,实时路径规划器可以在检测到障碍物后计算出新的路径,控制器会沿着这条路径运行。
在这个例子中,使用不同的方法,它利用MPC的处理明确的约束能力的优势。当检测到障碍物时,它定义了道路(在约束条件)上的一个区域,自我车子不能预测水平期间输入。在接下来的控制区间,该区域被重新定义基础上,自我汽车和障碍物的新位置,直到传球完成。
要定义,以避免区域,使用以下混合输入/输出约束:
E*u + F*y <= G
哪里ü
被操纵的变量是什么ÿ
为输出变量向量。你可以更新约束矩阵Ë
,F
,G
当控制器正在运行。
第一个约束是上的上限(这三车道的道路上)。
E1 = [0 0];F1 = [0 1 0 0];G1 / 2 =巷宽*车道;
第二个约束是上的下界(这三车道的道路上)。
E2 = [0 0];F2 = [0 -1 0 0];G2 =巷宽*车道/ 2;
第三个制约因素是避开障碍物。即使没有障碍在正常工作条件检测,必须在此处添加一个“假”的约束,因为你不能在运行时改变约束矩阵的尺寸。对于假约束,使用约束用相同的形式作为第二约束。
E3 = [0 0];F3 = [0 -1 0 0];G3 / 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 =零(长度(T),1);saveIntercept =零(长度(T),1);ympc =零(长度(T),尺寸(CD,1));UMPC =零(长度(T),大小(BD,2));
运行仿真。
为k = 1:长度(T)%获得新的植物模型和|k|区间的产量测量值。[AD,BD,CD,DD,U,Y,X,DX] = obstacleVehicleModelDT(TS,X,U);测量=镉* X + DD * U;ympc(K,:) =测量;确定车辆是否看到障碍物,并更新混合状态检测到障碍时的% I/O约束。检测= obstacleDetect (x,障碍,巷宽);[E, F, G, saveSlope (k), saveIntercept (k)] =...obstacleComputeCustomConstraint(X,检测,障碍物,laneWidth,泳道);%准备新工厂模型和自适应MPC标称条件。newPlant = ss(广告、Bd、Cd、弟弟、“t”、Ts);newNominal =结构(“U”U“Y”,Y,“X”,X,'DX',DX);准备新的混合I/O约束。选择= mpcmoveopt;选项。CustomConstraint =结构(“E”, E,“F”F‘G’,G);%计算最佳移动使用更新后的植物,标称条件,%和约束。(u,信息)= 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');轴([0 ympc(结束,1)* -laneWidth泳道/ 2 laneWidth *泳道/ 2])%重置轴
MPC控制器在没有人工干预的情况下成功完成任务。
打开Simulin金宝appk模型。避障系统包含多个组件:
植物模型生成器:产生新的植物模型和标称值。
障碍物检测:检测障碍物(不含激光雷达传感器)。
约束生成器:产生新的混合I/O约束。
自适应MPC:控制避障动作。
mdl =“mpc_ObstacleAvoidance”;open_system (mdl) sim (mdl)
模拟结果与命令行结果相同。为了支金宝app持快速原型工作流程,您可以为避障系统中的模块生成C/ c++代码。
bdclose (mdl)