系列文章目录

---

前言

本例演示了如何使用运动规划器在包含障碍物的环境中为挖掘机规划路径。在此示例中，您将以运动树的简化形式为挖掘机建模，然后使用基于采样的运动规划器确定挖掘机在存在障碍物的两个姿势之间的可行路径。在 Simscape™ 多体™ 模型中模拟结果。

---

一、获取挖掘机模型

本例使用 Simscape 中的挖掘机模型。该示例基于挖掘机（Simscape 多体）示例中的现有模型，该示例模拟了 PID 控制下的挖掘机。该模型将一组参考轨迹的输入传递给由液压活塞驱动的关节。控制器确保关节达到正确的角度，以执行参考轨迹。您可以使用此代码打开此模型：

open_system("sm/sm_backhoe")

本示例修改了模型，使其接受来自规划路径的轨迹，并在工作空间中添加了一个障碍物，该障碍物代表挖掘机工作区域中的一堵墙。目标是计算出一条计划路径，提供给仪器模型，使其执行无碰撞轨迹。

open_system("sm_backhoe_with_environment")

为初始轨迹值和以秒为单位的轨迹总长度初始化运行时参数。

actPath = ones(4,60); % Initial Trajectory values
maxTime = 20;          % Length (s) of the planned trajectory

二、创建运动规划模型

原始挖掘机是一个复杂的多域模型，其中有许多相互关联的链接。为了有效规划该系统的运动路径，可以创建并使用一个简化的、基于树形结构的挖掘机模型。这个简化的挖掘机是一个运动链，有四个外旋关节，与相关 Simulink® 模型中的四个 PID 控制角相对应。简化模型必须具备以下特性：

• 运动树结构的尺寸必须与原始模型相同。
• 关节限制必须与原始模型一致。
• 碰撞对象必须对原始模型的车身进行保守估计。

运动树必须至少包含四个关节，与挖掘机上的四个驱动关节相对应。一个旋转接头使机械臂横向旋转，三个旋转接头使挖掘机垂直移动铲斗。

在原始挖掘机中，液压活塞的回路闭合限制了这些接头的运动。但是，简化树模型中不存在这些连接，因此必须使用约束直接应用关节限制。

树必须有碰撞体，这些碰撞体代表不得与障碍物碰撞的元素。在这里，碰撞体被建模为包含所有相关运动部件的棱柱和圆柱。由于出于规划目的，连杆已被简化，因此碰撞体的尺寸略微偏大，以确保在规划过程中进行保守的碰撞估计。

 

开始创建满足这些要求的树。首先，直接从原始 Simscape 模型导入，然后添加连接限制和碰撞。有关此过程的详细信息，请参阅 helperImportAndCreateBackhoeRBT 辅助函数的文件。 

[backhoeRBT,activeConfigIdx] = helperImportAndCreateBackhoeRBT;

获取挖掘机的原点配置，并对其进行更新，将受控角度设置为非零值。

configToShow = homeConfiguration(backhoeRBT);
configToShow(activeConfigIdx) = [0 -pi/4 pi/12 -pi/24];

将结果可视化，并调整视图以适应。

ax = show(backhoeRBT,configToShow,Collisions="on"); 
title("Imported Rigid Body Tree with Collisions")
xlim([-8.5 14.5])
ylim([-17 6.5])
zlim([-11 12])
ax.Position = [0.1300 0.1100 0.7750 0.7150];
view(-60,27.5)

三、创建运动规划器

机器人建模完成后，就可以创建运动规划器了。运动规划器接受代表挖掘机的刚体树和代表环境的碰撞对象单元数组。要演示运动规划器，环境必须包含一堵墙作为碰撞框。

创建碰撞盒，将其添加到环境单元数组中，并改变其位置。

env = {collisionBox(.5,10,5)};
env{1}.Pose = trvec2tform([0 -17 -2]);

创建运动规划器。使用 manipulatorRRT 对象，该对象旨在为具有树状结构的机器人规划运动。

planner = manipulatorRRT(backhoeRBT,env);

接下来，配置规划器。根据任务调整连接距离和验证距离。确定这些值需要反复试验。

planner.MaxConnectionDistance = 1;
planner.ValidationDistance = 0.03;

由于刚体树的运动部件相对简单，而且其运动已经受到关节限制的约束，因此不太可能发生自碰撞。在这种情况下，可以通过取消自碰撞检查来提高规划器的效率。将规划器设置为忽略自碰撞。

planner.IgnoreSelfCollision = true;
planner.SkippedSelfCollisions = "parent"; % Set parameter explicitly to avoid compatibility warning

四、设置规划问题

定义起始姿势和目标姿势。规划器试图在这两个位置之间找到一条无碰撞的路径。

% Set initial and final position targets, using the active configuration
% index to ensure only controlled joints are set (other values are
% arbitrary)
initialState = [pi/8 -pi/4 0 0];
qStart = backhoeRBT.homeConfiguration;
qStart(activeConfigIdx) = initialState;

targetState = [-pi/8 -pi/12 -pi/4 0];
qGoal = backhoeRBT.homeConfiguration;
qGoal(activeConfigIdx) = targetState;

想象环境以及挖掘机的起始和目标姿势。

figure
ax = show(backhoeRBT,qStart,Collisions="on");
title("Planning Problem")
hold on
show(backhoeRBT,qGoal,Collisions="on",PreservePlot=true,Parent=ax); 
show(env{1},Parent=ax);
view(62,16)
axis equal
hold off

五、规划无碰撞路径

设置随机种子，以实现可重复性，然后运行规划器计算无碰撞路径。 

rng(20); % For repeatability
path = plan(planner,qStart',qGoal');

由于规划器的设计，最初返回的路径可能效率不高，看起来有些随机。使用缩短例程对路径进行迭代，找到更优化的解决方案。然后在规划的姿势之间进行内插，使结果更加平滑。

shortenedPath = shorten(planner,path,20);
smoothPath = interpolate(planner,shortenedPath,5);

在 MATLAB 中可视化规划路径。该输出显示了规划器使用简化模型和环境的结果。

figure
ax = show(backhoeRBT,qStart,Collisions="on");
xlim([-5 5])
ylim([-25 5])
zlim([-4 10])
drawnow
hold on
show(env{1},Parent=ax);
pause(0.5)
rc = rateControl(10);
for i = 1:size(smoothPath,1)
    show(backhoeRBT,smoothPath(i,:)',Parent=ax,Collisions="on",FastUpdate=true,PreservePlot=false);
    waitfor(rc);
end
hold off

六、在 Simulink 中模拟计划路径

返回 Simulink 模型，使用采样的计划路径作为输入执行模型。由于 Simulink 模拟的是随时间变化的模型，因此必须将采样路径转换为平滑轨迹。您可以使用 "最小跃变多项式轨迹 "块来实现。

生成的模型会产生一个无碰撞的轨迹，你可以用现有的挖掘机模型来执行。 

% Get the waypoints for the actuators & provide them to the model
actPath = smoothPath(:,activeConfigIdx)';

% Suppress benign warnings
mutedWarnings = helperMuteBenignWarnings();
for i = 1:numel(mutedWarnings)
    restoreWarnings{i} = onCleanup(@()warning(mutedWarnings{i}));
end

open_system("sm_backhoe_with_environment")
sim("sm_backhoe_with_environment")

% Clear the warning object
clear restoreWarnings;

 

七、可能的变化和扩展

在本例中，您将一个复杂系统建模为一棵树，并为该树规划了一条无碰撞路径。然后，您在 Simulink 中以单个连续轨迹的形式执行了规划路径，并对关节角度进行了控制。

7.1 使用允许更高保真系统建模的规划器

本示例中的方法之所以高效，是因为其建模的系统可以作为树形建模，而不会明显降低精度。这样，运动规划器就能计算前向运动学，而无需加入运动学约束，因为运动学约束的计算成本很高。使用树形结构还可以使用运动规划器。不过，对于保真度要求较高的系统，您可能需要利用运动学约束对整个系统进行建模。在这种情况下，请使用允许通用系统模型的规划器。

操纵器状态空间（manipulatorStateSpace）和操纵器碰撞体验证器（manipulatorCollisionBodyValidator）对象提供了接口，使用户能够为受约束刚体树对象规划路径。有关约束路径规划的更多信息，请参阅使用状态空间为操纵器规划带有末端执行器约束的路径。通过这些对象，您可以使用广义逆运动学来执行运动学约束，从而实现更复杂的规划器配置。这种复杂性对于复杂系统或狭小环境中的系统来说非常重要。

7.2 在 Simulink 中执行前的平滑轨迹

在本示例中，通过直接向轨迹生成器提供路径，将规划好的几何路径缩放到 Simulink 中。然而，生成的轨迹并不十分平滑。您可以使用缩短和内插函数对轨迹进行迭代，从而进一步平滑轨迹。此外，您还可以以较低的速率对输出路径重新采样，以生成更平滑的轨迹。在环境拥挤程度较低或对碰撞进行保守估计的情况下，可以使用这些平滑过程。