系列文章目录
前言
本示例展示了如何使用 “浮动 ”类型的刚体关节（rigidBodyJoint）和操纵器RRT 为浮动基座系统规划无碰撞的几何路径。浮动基座系统的基座带有一个 “浮动 ”关节，可以在空间自由平移和旋转，因此具有六个自由度。本示例以安装了机械臂的四旋翼无人机为例说明浮动基座系统。
要使用固定基座的刚体树（rigidBodyTree）对象对浮动基座系统进行建模，必须将浮体定义为通过 “浮动 ”关节连接到固定基座的刚体对象。请注意，这种建模方式无法对浮动基座系统进行逆运动学建模。要为浮动基座系统建模以进行逆运动学计算，请参阅浮动基座机器人的逆运动学。
一、将无人飞行器创建为浮动基座
首先，创建一个空的刚体树和一个带有浮动关节的刚体。
uavWithArm = rigidBodyTree(DataFormat="row");
uav = rigidBody("uav");
uav.Joint = rigidBodyJoint("uav_base_floating_joint","floating");通过读取多旋翼无人机 STL 文件，以三角测量对象的形式获取四旋翼无人机的几何图形。
tr = stlread("multirotor.stl");缩放几何图形，使其尺寸更精确地反映真实四旋翼无人机的大小。
scaledtr = triangulation(tr.ConnectivityList,tr.Points/200);
trimesh(scaledtr);
title("UAV Triangulation")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
axis equal
[az,el] = view;请注意，碰撞几何体是非凸的。要为碰撞检测精确地模拟此几何体，可使用 collisionVHACD 函数将其分解为近似凸网格的复合体。
options = vhacdOptions("IndividualMesh",MaxNumConvexHulls=8,MaxNumVerticesPerHull=50);
compositeMeshes = collisionVHACD(scaledtr,options);使用 addCollision 将每个分解网格添加到刚体上。
for i = 1:length(compositeMeshes)
addCollision(uav,compositeMeshes{i});
end将 UAV 刚体添加到浮动基座刚体树中，并将 UAV 可视化。
addBody(uavWithArm,uav,uavWithArm.BaseName);
show(uavWithArm,Collisions="on");
title("UAV Collision Mesh")
axis equal
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
view([az,el])
二、在无人飞行器上安装机械臂
为 Robotis 的开源机械臂加载刚体树。
openmanip = loadrobot("robotisOpenManipulator",DataFormat="row");要安装到无人飞行器底部的机械臂上，请设置机械臂第一个主体的固定变换，使机械臂在 Y 轴上旋转 pi 弧度，并应用一个小的 Z 轴平移偏移。Z 轴偏移是为了避免自碰撞。或者，也可以指定规划器的 SkippedSelfCollisions 属性，在自碰撞检查时忽略链接对。详情请参阅示例 “跳过特定体对间的自碰撞检查”。
fixedTF = eul2tform([0 pi 0])*trvec2tform([0 0 1e-4]);
setFixedTransform(openmanip.Bodies{1}.Joint,fixedTF);将机械臂刚体树模型安装到无人飞行器浮动底座上，并将无人飞行器与安装的机械臂可视化。
addSubtree(uavWithArm,"uav",openmanip);
show(uavWithArm,Collisions="on",Frames="off");
title("UAV With Mounted Robotic Arm")
axis equal
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
view(225,15)
三、运动规划问题
运动规划问题是要找到一条从起始关节构型到预期目标构型的无碰撞路径。为此，您必须定义问题的这些方面：
- 起始关节构型
- 目标关节构型
- 无人机碰撞模型
- 环境中的障碍物
- 机器人运动学约束
3.1 起点和目标关节构型
无人机的关节构型代表了无人机在世界中的姿态。因此，将描述无人机在世界中姿态的构型向量定义为 ，其中，
是刚体 “uav ”上的浮动关节相对于固定基座的方向，用 1-4 单位四元数表示；
是刚体 “uav ”上的浮动关节相对于固定基座的位置，用 1-3 向量表示；
是机器人手臂 openmanip 的关节构型向量，用 1-6 向量表示。
将浮动体 “uav ”的起始构型设置为 xyz 坐标 [0 0 -1] 和机械臂第三个关节的-pi/3 rad。
qstart = homeConfiguration(uavWithArm);
qstart(5:7) = [0 0 -1];
qstart(end-3) = -pi/3;使用起始构型并对其进行修改，得到目标构型。将主体 “uav ”的目标构型设置为 xyz 坐标 [2 0 2]，方向为 [pi/2 0 0] 欧拉 ZYX。
qgoal = qstart;
qgoal(5:7) = [2 0 2];
qgoal(1:4) = eul2quat([pi/2 0 0]);3.2 在环境中添加障碍物
使用碰撞盒创建环境，并设置碰撞盒的位置，使其在起点和目标构型之间形成一堵墙。
env={collisionBox(0.5,4,3,Pose=trvec2tform([1.0 0.0 -1.3])),...
collisionBox(0.5,4,1.5,Pose=trvec2tform([1.0 0.0 1.8]))};可视化规划问题。显示碰撞几何图形并隐藏无人飞行器上的坐标系。
figure
show(uavWithArm,qstart,Collisions="on",Frames="off");
hold on
showCollisionArray(env);
show(uavWithArm,qgoal,Collisions="on",Frames="off");
title("Planning Problem")
view(0,0)
hold off
3.3 机器人运动学约束
在本示例中，设置与 “uav ”刚体相关的浮动关节的平移位置限制（PositionLimits），使其在工作空间坐标系的 X 轴[-1,3]、Y 轴[-2,2]和 Z 轴[-2.5,2.5]的笛卡尔边界（例如物理安全笼）内飞行。请注意，浮动刚体关节的旋转限值无法设置，且为 NaNs。
uavBody=getBody(uavWithArm,"uav");
uavBody.Joint.PositionLimits(5:end,:)=[-1,3;-2,2;-2.5,2.5];四、查找无碰撞路径
查找起始关节和目标关节构型之间的无碰撞路径。为了找到这样一条路径，本示例使用操纵器RRT 来规划一条几何无碰撞路径。
rrt = manipulatorRRT(uavWithArm,env, ...
MaxConnectionDistance=0.5, ...
ValidationDistance=0.1, ...
SkippedSelfCollisions="parent");为随机数生成器加种子，使用 RRT 规划路径，缩短输出规划，并对缩短后的路径进行插值，使其更加平滑。
rng(0,"twister");
plannedpath = plan(rrt,qstart,qgoal);
shortenedPath = shorten(rrt,plannedpath,40);
rrtpath = interpolate(rrt,shortenedPath,10);五、可视化计划路径
以动画形式可视化安装了机械臂的无人机的计划几何路径。
show(uavWithArm,qstart,Collisions="on");
title("UAV Flight Animation")
view(0,0)
hold on
plot3(rrtpath(:,5),rrtpath(:,6),rrtpath(:,7),"k-",LineWidth=1)
showCollisionArray(env);
rc = rateControl(10);
for i = 1:1:size(rrtpath,1)
show(uavWithArm,rrtpath(i,:),...
PreservePlot=false,...
FastUpdate=true, ...
Frames="off", ...
Collisions="on");
waitfor(rc);
view(0,0)
end
hold off
请注意，机械臂也会沿路径移动，以避免与环境中的障碍物相撞。绘制计划路径中机械臂的关节角度。
plot(rrtpath(:,8:end),"-")
title("Robotic Arm Joint Angles")
xlabel("Time (s)")
ylabel("Joint Angle (rad)")
legend("Joint "+string(1:6))
六、下一步
6.1 为运动计划添加约束条件
请注意，规划的几何路径并没有对无人机施加任何方向限制，插值状态也没有施加运动学约束以实现可行的轨迹。要在几何路径中添加此类运动学约束，可使用从操纵器状态空间（manipulatorStateSpace）派生的自定义导航状态空间（nav.StateSpace）。请参考 “使用机械手状态空间的末端执行器约束规划路径 ”文档示例。
