基于RRT规划器和MPC跟踪控制器的平行泊车

这个示例使用:

这个例子展示了如何通过使用RRT星规划器生成路径和使用非线性模型预测控制(NLMPC)跟踪轨迹来平行停放一辆自我汽车。

停车环境

在本例中，停车环境包含一辆自我车辆和六个静态障碍。这些障碍包括四辆停着的汽车、路边的道路和道路上的一条黄线。自我交通工具的目标是停在一个目标姿势，而不与任何障碍相撞。自我车辆姿态的参考点位于后桥的中心。

自我交通工具有两个轴和四个轮子。定义自我车辆参数。

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

自我载具从以下初始姿势开始。

X的位置7米
Y的位置3．1米
偏航角0rad

egoInitialPose = [7, 3.1, 0);

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

X位置等于轴距长度的一半
Y的位置0米
偏航角0rad

egoTargetPose = (-distToCenter, 0, 0);

想象停车环境。指定可视化工具采样时间0．1年代。

电视= 0.1;helperSLVisualizeParking (egoInitialPose 0);

在可视化中，四辆停放的车辆是中间的橙色盒子。底部的橙色边界是道路的路边，顶部的橙色边界是道路上的黄线。

自我的汽车模型

为了解决停车问题，车辆以低速行驶。本例使用带有前转向角的运动自行车模型来解决车辆停放问题。自我载具的运动可以用下列方程式来描述。

$\begin{array}{l} x_{}^{˙} ＝ v \cdot 因为（ ψ ） \\ y_{}^{˙} ＝ v \cdot 罪（ ψ ） \\ ψ_{}^{˙} ＝ \frac{v}{b} \cdot 棕褐色（ δ ） \end{array}$

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

RRT Star的路径规划

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

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)自我车辆的速度是米/秒，和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函数parkingRRTMex，以加速仿真。代码生成成功。MEX函数parkingRRTMex生成成功。

运行模拟前初始化数据。

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

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

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

当使用NLMPC控制器时，绘制仿真结果并进行动画处理。跟踪结果与路径规划器提供的参考轨迹相匹配。

plotAndAnimateParkingRRT (p xRef xTrackHistory uTrackHistory);

图中包含3个轴对象。坐标轴对象1包含2个line类型的对象。这些对象表示RRT、NLMPC。坐标轴对象2包含两个line类型的对象。这些对象表示RRT、NLMPC。坐标轴对象3包含两个line类型的对象。这些对象表示RRT、NLMPC。

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

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

在Simulink中运行闭环仿真

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

mdl =“mpcVDAutoParkingRRT”；open_system (mdl)

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

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

模拟模型。

sim (mdl)

ans =仿真软件。SimulationOutput: tout: [7662x1 double] SimulationMetadata: [1x1 Simulink. txt]SimulationMetadata] ErrorMessage: [0x0 char]

检查自我载具的姿态和控制范围。仿真结果与MATLAB仿真结果相似。自我车辆已经成功地停在目标姿势，没有与任何障碍物发生碰撞。

结论

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

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