平行停车使用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);

图自动平行泊车包含一个坐标轴对象。坐标轴对象与标题平行停车包含20个对象类型的矩形,线,补丁,多边形。

在可视化,四个停放车辆中间橙色的框。底部边界道路路边和顶部的橙色橙边界上的黄线。

自我的汽车模型

对于停车问题,车辆在低速。这个示例使用运动自行车模型与前面车辆停车问题转向角。自我的运动车辆可以使用以下方程进行描述。

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

在这里, $(x, y)$ 表示车辆的位置 $ψ$ 表示汽车的偏航角。的参数 $b$ 代表汽车的轴距。 $(x, y, ψ)$ 车辆的状态变量是态函数。的速度 $v$ 和操舵角 $δ$ 车辆的控制变量是态函数。车辆状态函数实现parkingVehicleStateFcnRRT。

从RRT路径规划

配置的状态空间规划师。在这个例子中,自我车辆是一个三元素的状态向量,(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)

图自动平行泊车包含一个坐标轴对象。坐标轴对象与标题平行停车包含22个对象类型的矩形,线,补丁,多边形。

设计非线性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控制器的功能。

指定初始自我车辆状态。

x = egoInitialPose ';

指定初始控制输入。

u = (0, 0);

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

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

建立一个墨西哥人函数模拟控制器。

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

生成墨西哥人从非线性函数“parkingRRTMex”MPC加快仿真速度。代码生成成功。墨西哥人“parkingRRTMex”成功地生成函数。

运行仿真之前初始化数据。

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

在MATLAB运行闭环仿真使用墨西哥人的功能。

为ct = 1: Tsteps%的国家xk = x;%计算最优控制与墨西哥人功能onlinedata。ref = Xref (ct: min (ct + pTracking-1, Tsteps):);[mv, onlinedata信息]= mexfcn (xk, mv, onlinedata);%实现第一次最优控制移动和更新植物状态。ODEFUN = @ (t, xk) parkingVehicleStateFcnRRT (xk, mv);[吹捧,你]=数值([0 t], ODEFUN xk);x =你(最终:)”;%保存植物状态显示。xTrackHistory = [xTrackHistory x];% #好< * AGROW >uTrackHistory = (uTrackHistory mv);结束

情节和动画仿真结果在使用NLMPC控制器。跟踪结果匹配的参考轨迹路径规划。

plotAndAnimateParkingRRT (p xRef xTrackHistory uTrackHistory);

图自动平行泊车包含一个坐标轴对象。坐标轴对象与标题平行停车包含163个对象类型的矩形,线,补丁,多边形。

图包含3轴对象。坐标轴对象1包含2线类型的对象。这些对象代表RRT NLMPC。坐标轴对象包含2线类型的对象。这些对象代表RRT NLMPC。坐标轴对象3包含2线类型的对象。这些对象代表RRT NLMPC。

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

跟踪误差∞范数在x (m), y (m)和θ(度):0.0538,0.0538,1.3432最终控制输入的速度(米/秒)和转向角(度):0.0001,0.0981

在仿真软件运行闭环仿真金宝app

在仿真软件来模拟NLMPC控制器®,使用金宝app非线性MPC控制器块。对于这个示例,来模拟自我车辆,使用汽车的身体3自由度侧块,这是一个自行车模型(自动驾驶工具箱)块。

mdl =“mpcVDAutoParkingRRT”;open_system (mdl)

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

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

模拟模型。

sim (mdl)

图自动平行泊车包含一个坐标轴对象。坐标轴对象与标题平行停车包含160个对象类型的矩形,线,补丁,多边形。

ans =仿金宝app真软件。SimulationOutput:吹捧:x1双[7662]SimulationMetadata: [1 x1仿真软件。金宝appSimulationMetadata] ErrorMessage (0 x0 char):

自我检查车辆构成和控制范围。仿真结果与MATLAB仿真。自我碰撞车辆停在目标构成成功没有任何障碍。

结论

这个例子展示了如何如何平行泊车自我汽车通过生成路径使用RRT明星计划和跟踪轨迹使用非线性MPC控制器。控制器自我导航车辆目标停车位没有碰撞与任何障碍。

%启用消息显示mpcverbosity (“上”);%关闭仿真软件模金宝app型bdclose (mdl)%关闭动画情节f = findobj (“名字”,“自动平行泊车”);关闭(f)