这个例子展示了如何使用自适应MPC使车辆(自我车)遵循参考速度和避免在车道上的障碍。为此,您需要在运行时更新工厂模型和线性混合输入/输出约束。
具有避障(或辅助通行)功能的车辆有一个传感器,如激光雷达,可以测量车辆与前方和同一车道上障碍物的距离。障碍物可以是静态的,如一个大坑,也可以是移动的,如缓慢移动的车辆。司机最常见的机动方式是暂时移到另一条车道,驶过障碍物,然后再移回原来的车道。
作为自动驾驶体验的一部分,避障系统可以在没有人为干预的情况下进行机动操作。在这个例子中,您设计了一个避障系统,利用油门和转向角度使小轿车绕过车道上的静态障碍物。该系统采用自适应模型预测控制器,在每个控制区间更新预测模型和混合输入/输出约束。
“自我”车是长5米、宽2米的长方形。模型有四种状态:
-汽车中心的全球X位置
-汽车中心的全球Y位置
-汽车的航向角(0
朝东时,逆时针为正)
-赛车的速度(正)
有两个被操纵的变量:
-油门(加速时为正,减速时为负)
-转向角度(与车对齐时为0,逆时针为正)
使用一个简单的非线性模型来描述自我汽车的动力学:
利用非线性状态空间模型的解析雅可比矩阵来构造标称工作点的线性预测模型。
在哪里为车长。
假设所有的状态都是可测量的。在名义上的工作点,自我汽车以恒定的速度向东行驶20.
米每秒。
V = 20;x0 = [0;0;0;V];情况= [0;0);
采用零阶保持器方法对连续时间模型进行离散obstacleVehicleModelDT
函数。
t = 0.02;(广告、Bd、Cd, Dd, U, Y, X, DX) = obstacleVehicleModelDT (Ts, x0,情况);dsys = ss(广告、Bd、Cd、弟弟、“t”、Ts);dsys。InputName = {“节流”,“δ”};dsys。StateName = {“X”,“Y”,“θ”,“V”};dsys。OutputName = dsys.StateName;
在这个例子中,假设:
这条路是直的,有三条车道。
每条车道四米宽。
当不超车时,自负的汽车在中间车道行驶。
在不失一般性的情况下,自我汽车只从左侧(快速)车道通过障碍。
车道= 3;巷宽= 4;
在这个例子中,障碍物是一个不移动的物体,位于中心车道的中间,大小与ego汽车相同。
障碍=结构;障碍。长度= 5;障碍。宽度= 2;
把障碍50
沿着这条路走几米远。
障碍。X = 50;障碍。Y = 0;
在障碍物周围创建一个虚拟安全区,这样当自我汽车通过障碍物时就不会太靠近它。安全区以障碍为中心,并且有:
长度等于两辆车的长度
两条车道的宽度
障碍。safeDistanceX = obstacle.Length;障碍。safeDistanceY =巷宽;障碍= obstacleGenerateObstacleGeometryInfo(障碍);
在本例中,假设激光雷达设备可以探测到障碍物30.
在车辆前面几米。
障碍。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和)的权重设置为0来选择Y位置和速度。这样做可以让这些其他输出的值浮动。
mpcobj.Weights.OutputVariables = [0 30 0 1];
用标称运行状态更新控制器。对于离散时间设备:
U =情况
X = x0
Y = Cd*x0 + Dd*u0
DX = Ad*X0 + Bd*u0 - X0
mpcobj.Model.Nominal =结构(“U”U“Y”, Y,“X”, X,“DX”, DX);
有不同的策略使自我汽车避开道路上的障碍。例如,实时路径规划器可以在检测到障碍物后计算出一条新的路径,控制器沿着这条路径运行。
在本例中,使用一种不同的方法,利用MPC的能力显式地处理约束。当一个障碍物被检测到时,它定义了道路上的一个区域(在约束条件下),在预测的视界内自我汽车不能进入。在下一个控制区间,根据自我车和障碍物的新位置重新定义区域,直到通过完成。
要定义要避免的区域,请使用以下混合输入/输出约束:
E*u + F*y <= G
在哪里u
操纵变量是向量和吗y
为输出变量向量。你可以更新约束矩阵E
,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 =情况;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|的产量测量值。(广告、Bd、Cd, Dd, U, Y, X, DX) = obstacleVehicleModelDT (Ts, X, U);= Cd * x + Dd * u;: ympc (k) =测量”;确定车辆是否看到障碍物,并更新混合当检测到障碍时,% I/O约束。检测= obstacleDetect (x,障碍,巷宽);[E, F, G, saveSlope (k), saveIntercept (k)] =...obstacleComputeCustomConstraint (x,检测、障碍、巷宽车道);为适应性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 ';%更新下一个迭代的植物状态x = Ad * x + Bd * u;结束mpcverbosity(状态);
绘制避障机动中自我车的轨迹(黑线)和第三个混合I/O约束(绿虚线)。
图(f)为k = 1:length(saveSlope) X = [0;50;100];Y = saveSlope(k)*X + saveIntercept(k);线(X, Y,“线型”,“——”,“颜色”,‘g’)结束情节(ympc (: 1) ympc (:, 2),“- k”);轴([0 ympc(end,1) -laneWidth*lanes/2)%重置轴
MPC控制器无需人工干预即可成功完成任务。
打开Simulin金宝appk模型。避障系统包含多个部件:
植物模型生成器:产生新的植物模型和标称值。
障碍物探测器:探测障碍物(不包括激光雷达传感器)。
约束生成器:生成新的混合I/O约束。
自适应MPC:控制避障机动。
mdl =“mpc_ObstacleAvoidance”;open_system (mdl) sim (mdl)
模拟结果与命令行结果相同。为了支金宝app持快速原型工作流,您可以为避障系统中的块生成C/ c++代码。
bdclose (mdl)