基于plannerControlRRT的拖拉机-挂车模型逆向运动规划

打开实时脚本

本例展示了如何使用基于运动学的规划器为具有复杂运动学的系统寻找全局路径规划解，金宝搏官方网站plannerControlRRT．这个例子被组织成三个主要部分:

基诺动态规划导论。
适应plannerControlRRT转向拖拉机-拖车系统。
利用运动学系统求无碰撞轨迹。

基诺动态规划导论

计划描述

传统的几何规划器，如RRT, RRT*和hybridA*，是快速和可扩展的算法，能够为各种各样的规划问题找到完整和最优的解决方案。金宝搏官方网站然而，一个权衡是，他们对规划空间做出的假设可能不适用于现实世界的系统。传统的几何规划器假设空间中的任意两个状态可以通过一条无残差的轨迹连接起来，并且几何规划器返回的路径可以被物理系统跟踪。

对于具有复杂运动学的系统，或那些不容易确定连接状态的封闭形式解的系统，使用基诺动力学规划器，例如金宝搏官方网站plannerControlRRT．基诺动态规划者以完整性换取规划灵活性，并利用系统自身的运动学模型和控制器来生成可行且可跟踪的轨迹。

为给定的系统进行计划plannerControlRRTFeature需要以下信息:

在规划空间中对状态进行抽样的实用程序。
用来估计连接两个状态所需成本的度量。
确定地将系统从一种状态传播到另一种状态的机制。
确定状态是否达到目标的实用程序。

本示例演示了如何为牵引车-拖车系统制定这些不同的实用程序，并将它们调整到规划基础设施中。这个预览显示了结果:

尝试生成MEX以加速碰撞检查genFastCC

代码生成成功。

% load preplannedscenario停车场景=负载(“parkingScenario.mat”）;planner = parkingScenario.planner;start = parkingScenario.start;目标=停车场景。%显示场景，回放轨迹显示(planner.StatePropagator.StateValidator);轴平等的持有在轨迹=计划(计划，开始，目标);demoParams = trajectory. statepropagator . statspace . truckparams;exampleHelperPlotTruck (demoParams,开始);显示开始配置exampleHelperPlotTruck (demoParams、目标);显示目标配置使用以前构建的实用程序规划路径Rng(1)轨迹=计划(计划，开始，目标);%动画轨迹exampleHelperPlotTruck(轨迹);持有从

基诺动态规划算法

如果你已经熟悉几何规划器，一个迭代的plannerControlRRT搜索算法应该看起来很熟悉，但有几个额外的步骤:

样本目标状态。
求近似值最近的邻居到搜索树中的目标状态。
生成驱动系统向目标移动的控制输入(或参考值)和控制持续时间。
传播(延伸)系统对样品，而验证沿传播轨迹的中间状态。返回有效的状态、控件和持续时间序列。
检查是否有返回州达到目标。
如果目标已经找到，退出。否则添加树的最终状态、初始控制、累积持续时间和目标。
(可选)采样目标配置和控制，重复步骤4、5和6所需的次数。

所需组件`plannerControlRRT`框架

要使计划程序适应特定的问题，必须实现自定义导航。StatePropagator而且导航。StateSpace类。中描述的步骤的映射基诺动态规划算法节指定负责的类和方法:

$问_{tgt} ＝ sampleUniform （空间）$
$［经销，犯错］＝距离（宣传者，问_{树} ，问_{tgt} ）$
$［ u ， N_{一步} ］＝ sampleControl （宣传者，问_{0} ， u_{0} ，问_{tgt} ）$
${［问_{1 ： n} ， U_{1 ： n} ， n_{{一步}_{1 ： n}} ］}_{有效的} ＝ propagateWhileValid （宣传者，问_{0} ， u_{0} ，问_{tgt} ， N_{一步} ）$
plannerControlRRT检查是否有返回的状态达到了目标。
默认情况下，退出规划或添加到搜索树由距离传播器的函数。您可以通过在构造过程中为规划器提供函数句柄来覆盖它。
默认情况下，每当规划器成功地将一个节点添加到树中时，它将尝试将这个新添加的状态传播到目标配置。对象可以在一行中出现多次，也可以完全禁用此目标传播NumGoalExtension规划器的属性。

有关更详细的函数语法、参数要求和示例，请参见plannerControlRRT，导航。StateSpace，而且导航。StatePropagator．

适应`plannerControlRRT`转向卡车拖车系统

本节演示如何制定、建模和控制一个两体卡车-拖车系统。该系统在反向运动时具有固有的不稳定性，因此传统几何规划器返回的路径不太可能在路径跟踪中产生良好的结果。要准确地为这个系统建模，您必须:

定义一个状态转换函数。
定义几何模型参数。
创建一个与参数化模型协调的控制律。
创建一个与参数化模型相协调的距离启发式。
将运动学模型、距离度量和控制律打包到自定义状态空间和状态传播器中。
为几个问题场景规划一个路径。

定义状态转换函数

该牵引车-挂车系统由两个刚体在一个连接处组成主要人物结，它被建模为一个转动关节。拖车的车身在后轴处与地面接触，拖车的前部由位于卡车轴中心线某处的挂车钩支撑。金宝app

跟踪配置(左，M < 0)， OverCenter配置(右，M > 0)

使用多级非线性MPC的例子，使用两体卡车-拖车系统的几何配置，表示为:

$问_{sys} ＝［ x_{2} y_{2} θ_{2} β ］$ ，

地点:

$x_{2}$ ——全球 $x$ -拖车后轴中心位置。
$y_{2}$ ——全球 $y$ -拖车后轴中心位置。
$θ_{2}$ -拖车方向的全局角度，其中0是东。
$β$ -卡车相对于拖车的方向，其中0是一致的。

为作规划用途，请附加方向标志， $v_{标志}$ ，到起始构型的总距离， ${年代}_{合计}$ 到状态向量，作为最终状态符号:

$问＝［ x_{2} y_{2} θ_{2} β v_{标志} {年代}_{合计} ］$

$v_{标志}$ -指示所需的控制模式(正向或反向)或所需的速度，确保系统以所需的方向向目标传播。
${年代}_{合计}$ -修改传播程序的行为距离传播器的函数。你可以修改传播器的TravelBias属性更改最近邻居比较包含或不包含根到节点代价的频率。接近于0的值会导致更快地搜索规划空间，但代价是建立更多的局部最优连接。接近1的值可以改进规划方案，但会增加规划时间。金宝搏官方网站

这个状态转换函数是TruckTrailerStateFcn．有关状态转换函数的完整推导，请参见exampleHelperStateDerivative，这是对N-trailer模型的简化[1]：

$\dot{问} （ l_{1} ， l_{2} ，米，问， u_{应用程序} ）＝［ \dot{x_{2}} \dot{y_{2}} \dot{θ_{2}} \dot{β} 0 v ］$

为简单起见，此模型处理 $u_{应用程序} ＝［ v ， α_{引导} ］$ 由于瞬时速度和转向角度命令应用于系统，由更高一级控制器生成。有关更多信息，请参见为稳定的向前和反向运动创建控制律．

定义几何模型参数并演示不稳定性

模型参数的选择对车辆的运动特性有很大的影响。创建一个描述牵引-拖车系统的结构，

struct = struct;truckParams。L1 = 6;卡车车身长度%truckParams。L2 = 10;拖车长度%truckParams。M = 1;%吊钩与卡车后轴沿车身的距离+ x轴

定义与可视化和碰撞检查相关的属性。

truckParams。W1 = 2.5;%卡车宽度truckParams。W2 = 2.5;拖车宽度%truckParams。Lwheel = 1;%车轮长度truckParams。车轮= 0.4;%轮宽

使用exampleHelperShowOpenLoopDynamics函数和指定的模型参数，利用其开环动力学传播系统状态，以证明内角在向前运动时自稳定，而在任何非零的反向运动时不稳定 $β_{0}$ ．

exampleHelperShowOpenLoopDynamics (truckParams);

为稳定的向前和反向运动创建控制律

本例采用多层模式切换控制器来控制卡车-拖车模型。在顶层，纯追踪控制器计算一个参考点， $P ＝［ X_{p} ， Y_{p} ］$ ，在当前姿势之间， $问_{我}$ ，目标状态， $问_{tgt}$ ．控制器有正向和反向两种模式。有关此控制法基础的信息，请参见[1]．对于向前运动，控制器计算一个转向角度，当保持恒定时，驱动卡车后桥沿与参考点相交的弧线前进:

${问_{1}}_{我} ＝ {［ x_{1 ，} y_{1} θ_{1} ］}_{我}$

$θ_{犯错} ＝量化（ X_{p} - x_{1} ， Y_{p} - y_{1} ） - θ_{1}$

$α_{前轮驱动} ＝ f （ θ_{犯错} ）$

倒车时，拖车后轴进入受控状态， $问_{2} ＝［ x_{2} ， y_{2} ， θ_{2} ］$ ．此外，由于系统在反向运动时固有不稳定，控制律将顶层控制器返回的转向角作为增益调度LQ控制器的参考值，LQ控制器寻求稳定车辆的内角:

$θ_{犯错} ＝量化（ {X_{p}}^{”} - x_{2} ， {Y_{p}}^{”} - y_{2} ） - θ_{2}$

$β_{d} ＝ - : （ \frac{2 l_{2} 罪（ θ_{犯错} ）}{R} ）$

$β_{裁判} ＝ β_{d} + K_{p} （ β_{d} - β ）$

LQ反馈稳定

在反向运动期间，您可以添加一个增益转向角度，驱动内角到一个平衡点。使用LQ控制器计算所需的转向角度和反馈增益。增益是代数里卡蒂方程的最优解，存储为依赖于所需转向角度的查找表。有关此反馈控制器的派生的更多信息，请参见exampleHelperCalculateLQGains：

定义LQR控制器的Q/R权重矩阵Q = 10;%重量驱动betaDot -> 0R = 1;%重量最小化转向角度命令导出几何转向极限并求解LQR反馈增益[alphaLimit,.．.千斤顶刀前的最大稳态转向角betaLimit,.．.千斤顶刀前最大内角alphaDesiredEq,.．.%稳定alpha域的采样角度alphaGain.．.% LQ增益对应于所需alpha] = examplehelpercalculatelqgain (truckParams,Q,R);

为卡车几何体添加限制truckParams。AlphaLimit = AlphaLimit;truckParams。BetaLimit = BetaLimit;

为纯追踪控制器指定前视距离，exampleHelperPurePursuitGetSteering，以及模型模拟器的速率和时间跨度信息，exampleHelperPropagateTruck．因为反向运动是不稳定的，并且您只能间接控制内角，所以在反向中指定更大的前视距离。这使系统有更多的时间来最小化虚拟转向误差，同时保持稳定的内角。

选择正向和反向前视距离。在这个例子中，使用两倍于前进距离的反向前进距离。的前向距离，它本身略长于%轴距。可以对这些参数进行调优对于给定几何图形，性能提高%。rFWD = truckParams.L1*1.2;rREV = rFWD*2;定义固定速率传播程序的参数，并将它们添加到%控制结构controlParams。米axVelocity = 3;% m / scontrolParams。StepSize = .1;% scontrolParams。米axNumSteps = 50;%#ok每次传播的最大步数

创建控件相关信息的结构。

在结构中存储增益和控制参数controlParams = struct(.．.“MaxSteer”alphaLimit,.．.% (rad)“收益”alphaGain,.．.% ()“AlphaPoints”alphaDesiredEq,.．.% (rad)“ForwardLookahead”rFWD,.．.% (m)“ReverseLookahead”rREV,.．.% (m)“MaxVelocity”3,.．.%(米/秒)“StepSize”, 0.1,.．.% (s)“MaxNumSteps”, 200年.．.% ()）;

定义距离启发式

定义一个距离启发式来近似系统状态空间中不同配置之间的代价。当规划器试图在树中找到与采样状态最近的节点时，状态传播器使用此选项。方法离线计算成本exampleHelperCalculateDistanceMetric函数将其存储在一个查找表中。

distanceHeuristic = exampleHelperCalculateDistanceMetric(truckParams,controlParams);

适应系统与运动学规划器的使用

将系统调整为运动学路径规划器可用的框架，plannerControlRRT．创建3个定制类:exampleHelperTruckStateSpace，exampleHelperTruckPropagator，而且exampleHelperTruckValidator，继承自导航。StateSpace,导航。StatePropagator,而且导航。StateValidator分别子类。

初始化状态空间

状态空间对象主要负责:

为计划器随机采样状态。
定义和执行轨迹内的状态限制。

定义xy -可搜索区域的限制。使用这些限制初始化状态空间 $β$ -由卡车拖车的几何属性决定的限制。

因为运动学规划器使用距离状态传播器的函数，用于最近邻搜索，离开状态空间距离方法定义。离开插入而且sampleGaussian状态空间的方法同样未定义。

定义可搜索区域的限制xyLimits = [-60 60;-40年40];构造我们的状态空间stateSpace = exampleHelperTruckStateSpace(xyLimits, truckParams);

初始化状态验证器

在本例中，状态验证器使用定向包围框(obb)来验证车辆是否与环境发生碰撞。卡车由两个矩形表示，它们的姿态由状态和包含在状态空间对象中的卡车的车辆几何形状定义。环境由一个矩形列表表示，矩形的位置、方向和尺寸以1 × n的结构指定。将包含环境的MAT文件加载到工作区中，并使用障碍和状态空间对象来构造状态验证器。

加载一组障碍负载(“Slalom.mat”）;使用状态空间和障碍列表构造状态验证器。validator = exampleHelperTruckValidator(stateSpace,obstacle);

初始化状态传播器

在运动学规划中，状态传播器对象负责:

估计状态之间的距离。
采样初始控件以在控制间隔内传播，或者在传播期间捕获控制输入作为低级控制器的参考值。
将系统传播到目标，并将弹道的有效部分返回给规划器。

为获得最佳效果，使用距离函数应该估计在状态之间传播时产生的轨迹的代价。

使用状态验证器、控制参数、%和距离查找表传播器= exampleHelperTruckPropagator(验证器，controlParams,distanceHeuristic);

构造计划器和计划路径

构造一个运动学路径规划器，它使用状态传播器来搜索两个构型之间的路径。

定义启动配置Start = [35 5 pi/2 0 0 0];定义目标配置，使卡车必须倒车到%的位置。目标= [-35 20 -pi/2 0 -1 nan];%显示问题图显示(验证器)保持在Configs = [start;目的];箭袋(配置(:1),配置(:,2),因为(配置(:,3)),罪(配置(:,3)),1。)

定义一个函数来检查计划器是否达到了真正的目标状态。goalFcn = @(规划师，q,qTgt)exampleHelperGoalReachedFunction(目标，规划师，q,qTgt);%施工规划师planner = plannerControlRRT(传播器，GoalReachedFcn=goalFcn,MaxNumIteration=30000);计划。NumGoalExtension = 3; planner.MaxNumTreeNode = 30000; planner.GoalBias = .25;

%搜索路径rng(0)[轨迹，树信息]=计划(计划，开始，目标)

弹道= navPathControl with properties: StatePropagator: [1x1 exampleHelperTruckPropagator] States: [23x6 double] Controls: [22x4 double] duration: [22x1 double] TargetStates: [22x6 double] NumStates: 23 NumSegments: 22

treeInfo =带字段的结构:IsPathFound: 1 ExitFlag: 1 NumTreeNode: 2201 NumIteration: 1196 PlanningTime: 10.9237 TreeInfo: [6x6602 double]

可视化路径和路径点exampleHelperPlotTruck(轨迹);持有从

而不是产生最优的解决方案，就像在使用多级非线性MPC的卡车和金宝搏官方网站拖车自动泊车的例子中产生的那些，这个规划器的优势在于它能够为各种各样的问题找到可行的轨迹。例如，在下面的场景中，生成的路径可以被使用，或者作为MPC求解器的初始猜测，有助于避免非凸问题空间中的局部极小值:

%负载场景障碍nonConvexProblem = load(“NonConvex.mat”）;更新验证器和状态空间验证器。障碍= nonconvexproblem .obstacle;validator. statspace . statebounds (1:2，:) = [-100 100;-40年40];图显示(validator)确定起点和目标start = [10 0 0 0 0 NaN 0];目标= [-10 0 PI 0 -10];%更新目标达到功能goalFcn = @(规划师，q,qTgt)exampleHelperGoalReachedFunction(目标，规划师，q,qTgt);计划。GoalReachedFcn = goalFcn;%关闭可选的每一步目标传播计划。NumGoalExtension = 0;增加目标采样频率计划。目标偏差= .25;平衡搜索与路径优化宣传者。TravelBias＝．5;%计划路径Rng(0)轨迹=计划(计划，开始，目标);可视化结果exampleHelperPlotTruck(轨迹);