采用RRT规划器和MPC跟踪控制器的平行泊车
这个例子展示了如何通过使用RRT星规划器生成路径并使用非线性模型预测控制(NLMPC)跟踪轨迹来平行泊车。
停车环境
在本例中,停车环境包含一辆自我车辆和六个静态障碍物。这些障碍包括四辆停放的车辆、路边的道路和道路上的黄线。自我车辆的目标是在不与任何障碍物碰撞的情况下停在目标位置。车辆姿态的参考点位于后轴的中心。
自我车辆有两个轴和四个轮子。定义自我车辆参数。
vdims =车辆尺寸;egoWheelbase = vdim .轴距;distToCenter = 0.5*egoWheelbase;
自我载具以以下初始姿势开始。
X位置
7
米Y位置
3.1
米偏航角
0
rad
egoInitialPose = [7,3.1,0];
将自我车辆的中心停在目标位置(X =0
, y =0
)使用以下目标姿态,它指定了后轴参考点的位置。
X位置等于轴距长度的一半
Y位置
0
米偏航角
0
rad
egoTargetPose = [-distToCenter,0,0];
想象停车环境。指定可视化器采样时间为0.1
年代。
Tv = 0.1;helperSLVisualizeParking (egoInitialPose 0);
在可视化中,四辆停放的车辆是中间的橙色盒子。下面的橙色边界是路边,上面的橙色边界是道路上的黄色线。
自我车辆模型
对于停车问题,车辆以低速行驶。本例采用带前转向角的运动自行车模型求解车辆停放问题。自我载具的运动可以用下面的方程来描述。
在这里,
表示车辆的位置和
表示飞行器的偏航角度。的参数
表示车辆的轴距。
是车辆状态函数的状态变量。的速度
转向角度
是车辆状态函数的控制变量。车辆状态函数在parkingVehicleStateFcnRRT
.
《RRT Star》中的路径规划
为计划器配置状态空间。在本例中,自我载体的状态是一个三元素向量[x y theta],其中 坐标单位为米,旋转角度单位为弧度。
Xlim = [-10 10];Ylim = [-2 6];Yawlim = [-3.1416 3.1416];Bounds = [xlim;ylim;yawlim];stateSpace = stateSpaceReedsShepp(bounds);stateSpace。MinTurningRadius = 7;
创建一个自定义状态验证器。规划器需要一个定制的状态验证器来启用自我车辆和障碍物之间的碰撞检查。
stateValidator = parkingStateValidator(stateSpace);
配置路径规划器。使用plannerRRTStar
作为规划器,并指定状态空间和状态验证器。为计划器指定其他参数。
planner = plannerRRTStar(stateSpace,stateValidator);计划。MaxConnectionDistance = 4;计划。ContinueAfterGoalReached = true;计划。MaxIterations = 2000;
使用配置的路径规划器规划从初始姿态到目标姿态的路径。为可重复性设置随机数种子。
rng (9,“旋风”);[pathObj,solnInfo] = plan(planner,egoInitialPose,egoTargetPose);
在停车环境上绘制树扩展图。
F = findobj(“名字”,“自动平行泊车”);Ax = gca(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
)为控制器。
Ts = 0.1;pTracking = 10;nlobjTracking。Ts = Ts;nlobjTracking。PredictionHorizon = pTracking;nlobjTracking。ControlHorizon = pTracking;
为被操作的变量定义约束。在这里,MV (1)
自我车辆的速度是米/秒,和MV (2)
是以弧度为单位的转向角度。
nlobjTracking.MV(1)。Min = -2;nlobjTracking.MV(1)。Max = 2;nlobjTracking.MV(2)。Min = -pi/6;nlobjTracking.MV(2)。Max = pi/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是OK的。雅可比矩阵。StateFcn是OK的。没有指定输出函数。假设预测模型中“y = x”。优化。CustomEqConFcn是OK的。分析了用户提供的模型、成本和约束函数。
在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;步骤=持续时间/Ts;Xref = [Xref (2: p + 1,:); repmat (Xref(最终,:),Tsteps-p, 1)];
在MATLAB中使用MEX函数进行闭环仿真。
为ct = 1:步骤%的国家Xk = x;使用MEX功能计算最优控制移动onlinedata。ref = Xref(ct:min(ct+ ptracing -1,Tsteps),:);[mv,onlinedata,info] = mexfcn(xk,mv,onlinedata);实现第一次最优控制移动和更新工厂状态。ODEFUN = @(t,xk) parkingVehicleStateFcnRRT(xk,mv);[TOUT,YOUT] = ode45(ODEFUN,[0 Ts], xk);x = YOUT(end,:)';保存植物状态以供显示。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中运行闭环仿真万博1manbetx
要在Simulink®中模拟NLMPC控制器,请使用万博1manbetx非线性MPC控制器块。对于这个例子,要模拟自我车辆,使用车体3DOF横向块,这是一个自行车模型(自动驾驶工具箱)块。
mdl =“mpcVDAutoParkingRRT”;open_system (mdl)
在模拟模型之前关闭动画图。
F = findobj(“名字”,“自动平行泊车”);关闭(f)
模拟模型。
sim (mdl)
ans = 万博1manbetxSimulink。SimulationOutput: tout: [7662x1 double] SimulationMetadata: [1x1 万博1manbetxSimulink. txt]SimulationMetadata] ErrorMessage: [0x0 char]
检查自我飞行器姿态和控制范围。仿真结果与MATLAB仿真相似。自我车辆已经成功地停在目标位置,没有与任何障碍物碰撞。
结论
这个例子展示了如何通过使用RRT星形规划器生成路径并使用非线性MPC控制器跟踪轨迹来平行泊车。控制器导航自我车辆到目标停车位而不与任何障碍物碰撞。
%启用消息显示mpcverbosity (“上”);关闭Simuli万博1manbetxnk模型bdclose (mdl)%关闭动画图F = findobj(“名字”,“自动平行泊车”);关闭(f)