停车环境

在这个例子中，停车环境包含一辆自我车和六个静态障碍物。障碍物包括四辆停在路边的车辆、道路边和道路上的一条黄线。自我车辆的目标是以目标姿态停车，而不撞上任何障碍。自我车辆姿态的参考点位于后桥的中心。

ego汽车有两个车轴和四个轮子。定义自我车辆参数。

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

自我载体以以下初始姿态开始。

egoInitialPose = [7, 3.1, 0);

将自我车辆的中心停在目标位置(X =0Y =0)使用以下目标位姿，指定后轴参考点的位置。

egoTargetPose = (-distToCenter, 0, 0);

想象一下停车的环境。指定可视化程序的示例时间０.１年代。

电视= 0.1;helperSLVisualizeParking (egoInitialPose 0);

在可视化中，四辆停着的车辆是中间的橙色盒子。下面的橙色边界是道路的边沿，上面的橙色边界是道路上的黄线。

自我的汽车模型

对于停车问题，车辆以低速行驶。这个例子使用一个带有前转向角度的运动学自行车模型来解决车辆停放问题。自我载体的运动可以用下面的方程来描述。

在这里, $（\mathit{x}，\mathit{y}）$表示车辆的位置和 $$\psi$$表示飞行器的偏航角。的参数 $$b$$表示车辆的轴距。 $$（x，y，\psi ）$$是车辆状态函数的状态变量。的速度 $\mathit{v}$和操舵角 $$\delta$$为车辆状态函数的控制变量。实现了车辆状态函数parkingVehicleStateFcnRRT．

路径规划从RRT星

配置规划器的状态空间。在这个例子中，自我载体的状态是一个三元素向量[x y]，带有 $$xy$$坐标以米为单位，旋转角度以弧度为单位。

Xlim = [-10 10];Ylim = [-2 6];Yawlim = [-3.1416 3.1416];边界= [xlim; ylim yawlim];stateSpace = stateSpaceReedsShepp(范围);stateSpace。MinTurningRadius = 7;

创建一个自定义状态验证器。规划器需要一个定制的状态验证器来启用自我车辆和障碍物之间的碰撞检查。

stateValidator = parkingStateValidator (stateSpace);

配置路径规划器使用plannerRRTStar作为计划器，并指定状态空间和状态验证器。为规划器指定其他参数。

规划师= plannerRRTStar (stateSpace stateValidator);计划。MaxConnectionDistance = 4;计划。ContinueAfterGoalReached = true;计划。MaxIterations = 2000;

使用配置的路径规划器规划从初始位姿到目标位姿的路径。为可重复性设置随机数种子。

rng (9,“旋风”）;[pathObj, solnInfo] =计划(规划师、egoInitialPose egoTargetPose);

在停车环境中规划树木扩展。

f = findobj (“名字”，“自动平行泊车”）;甘氨胆酸ax = (f);(ax,“上”）;情节(ax, solnInfo.TreeData (: 1), solnInfo.TreeData (:, 2),“——”）;%树扩张

生成轨迹pathObj通过插值适当数量的点。

p = 100;pathObj.interpolate (p + 1);xRef = pathObj.States;

在环境上画出路径。

情节(ax, xRef (: 1), xRef (:, 2),“b -”，“线宽”, 2)

设计非线性MPC跟踪控制器

创建非线性MPC控制器。为清晰起见，首先禁用MPC命令窗口消息。

mpcverbosity (“关闭”）;

创建nlmpc具有三个状态、三个输出和两个输入的控制器对象。

nlobjTracking = nlmpc(3、3、2);

指定样本时间(Ts)、预测视界(PredictionHorizon)，以及控制层(ControlHorizon)为控制器。

t = 0.1;pTracking = 10;nlobjTracking。Ts = t;nlobjTracking。PredictionHorizon = pTracking;nlobjTracking。ControlHorizon = pTracking;

为被操作的变量定义约束。在这里,MV (1)自我飞行器的速度是m/s吗MV (2)是以弧度为单位的转向角度。

nlobjTracking.MV(1)。Min = 2;nlobjTracking.MV(1)。Max = 2;nlobjTracking.MV(2)。Min = -π/ 6;nlobjTracking.MV(2)。Max =π/ 6;

为控制器指定调优权重。

nlobjTracking.Weights.OutputVariables = (1, 1, 3);nlobjTracking.Weights.ManipulatedVariablesRate = [0.1, 0.2];

自我车辆的运动由一个运动学自行车模型控制。指定控制器状态函数和状态函数雅可比矩阵。

nlobjTracking.Model.StateFcn =“parkingVehicleStateFcnRRT”；nlobjTracking.Jacobian.StateFcn =“parkingVehicleStateJacobianFcnRRT”；

指定控制输入的终端约束。速度和转向角度都将在最后为零。

nlobjTracking.Optimization.CustomEqConFcn =“parkingTerminalConFcn”；

验证控制器设计。

validateFcns (nlobjTracking randn (3,1), randn (2,1));

模型。StateFcn就可以了。雅可比矩阵。StateFcn就可以了。没有指定输出函数。假设预测模型中的y = x。优化。CustomEqConFcn就可以了。分析了用户提供的模型、成本和约束功能。

在MATLAB中进行闭环仿真

为了加快仿真速度，首先为NLMPC控制器生成MEX函数。

指定初始自我车辆状态。

x = egoInitialPose ';

指定初始控制输入。

u = (0, 0);

获取NLMPC控制器的代码生成数据。

[coredata, onlinedata] = getCodeGenerationData (nlobjTracking, x, u);

建立一个MEX函数来模拟控制器。

mexfcn = buildMEX (nlobjTracking,“parkingRRTMex”、coredata onlinedata);

从非线性MPC生成MEX函数“parkgrrtmex”以加速仿真。代码生成成功。MEX函数“parkgrrtmex”成功生成。

在运行模拟之前初始化数据。

xTrackHistory = x;uTrackHistory = u;mv = u;时间= 14;Tsteps = / Ts持续时间;Xref = [Xref (2: p + 1,:); repmat (Xref(最终,:),Tsteps-p, 1)];

利用MEX函数在MATLAB中进行闭环仿真。

为ct = 1: Tsteps%的国家xk = x;使用MEX函数计算最优控制动作onlinedata。ref = Xref (ct: min (ct + pTracking-1, Tsteps):);[mv, onlinedata信息]= mexfcn (xk, mv, onlinedata);实施第一次最优控制并更新工厂状态。ODEFUN = @(t,xk) parkingVehicleStateFcnRRT(xk,mv);[TOUT,YOUT] = ode45(ODEFUN，[0 Ts]， xk);x =你(最终:)”;保存植物状态以供显示。xTrackHistory = [xTrackHistory x];% #好< * AGROW >uTrackHistory = [uTrackHistory mv];结束

绘制和动画的仿真结果时，使用NLMPC控制器。跟踪结果与路径规划器给出的参考轨迹相匹配。

plotAndAnimateParkingRRT (p xRef xTrackHistory uTrackHistory);

跟踪误差无穷范数在x (m)， y (m)和theta (deg): 0.0538, 0.0538, 1.3432速度(m/s)和转向角度(deg)的最终控制输入:0.0001,0.0981

在Simulink中运行闭环仿真金宝app

在Simulink®中模拟NLMPC控制器，请使用金宝app非线性MPC控制器块。在这个例子中，为了模拟自我车辆，使用车身3DOF横向块，这是一个自行车模型(自动驾驶工具箱)块。

mdl =“mpcVDAutoParkingRRT”；open_system (mdl)

在模拟模型之前关闭动画情节。

f = findobj (“名字”，“自动平行泊车”）;关闭(f)

模拟模型。

sim (mdl)

ans =仿金宝app真软件。SimulationMetadata: [1x1 Simulink. out: out: [7658x1 double]金宝appSimulationMetadata]错误信息:[0x0 char]

检查自我飞行器的姿态和控制范围。仿真结果与MATLAB仿真结果基本一致。自我飞行器成功地停在目标位置，没有与任何障碍发生碰撞。

结论

这个例子展示了如何通过使用RRT星形规划器生成路径和使用非线性MPC控制器跟踪轨迹来并行停放自我汽车。控制器将自我车辆导航到目标停车位而不与任何障碍物相撞。

%启用消息显示mpcverbosity (“上”）;%关闭Simul金宝appink模型bdclose (mdl)关闭动画图f = findobj (“名字”，“自动平行泊车”）;关闭(f)