四旋翼机器人手臂路径规划
四旋翼机器人手臂路径规划
本示例展示如何使用 "floating" 类型 rigidBodyJoint 和 manipulatorRRT 为浮动基系统规划无碰撞几何路径。浮动基系统具有一个 “floating” 关节，该关节可以在空间中自由平移和旋转，因此具有六个自由度。本示例使用安装有机械臂的四旋翼无人机作为浮动基系统的示例。
要使用固定基座 rigidBodyTree 对象对浮动基座系统进行建模，您必须将浮动体定义为 rigidBody 对象，并通过 “floating” 关节将其连接到固定基座。请注意，您无法对以这种方式建模的浮动基座系统执行逆运动学计算。要为逆运动学计算建模浮动基座系统，请参阅具有浮动基座机器人的逆运动学。
创建无人机作为浮动基地
首先，创建一个空的刚体树和一个具有浮动关节的刚体。
uavWithArm = rigidBodyTree(DataFormat="row");
uav = rigidBody("uav");
uav.Joint = rigidBodyJoint("uav_base_floating_joint","floating");通过读取 multirotor STL 文件，获取四旋翼无人机的几何形状作为 triangulation 对象。
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
请注意，碰撞几何体是非凸的。为了精确建模该几何体以进行碰撞检查，请使用 collisionVHACD 函数将其分解为近似凸网格的复合体。
options = vhacdOptions("IndividualMesh",MaxNumConvexHulls=8,MaxNumVerticesPerHull=50);
compositeMeshes = collisionVHACD(scaledtr,options);
使用 addCollision 将每个分解的网格添加到刚体。
for i = 1:length(compositeMeshes)addCollision(uav,compositeMeshes{i});
end
将无人机刚体添加到浮基刚体树中，并可视化无人机。
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 加载 Open Manipulator 机械臂的刚体树。
openmanip = loadrobot("robotisOpenManipulator",DataFormat="row");
要将其安装到无人机底部的机械臂上，请设置机械臂第一个主体的固定变换，使其在 y 轴上旋转 pi 弧度，并在 z 轴上施加较小的平移偏移。z 轴偏移是为了避免自碰撞。或者，您可以指定规划器的 SkippedSelfCollisions 属性，指定在自碰撞检查时忽略的连杆对。更多详情，请参阅示例 Skip Self-Collision Checking Between Specific Body Pairs
Skip Self-Collision Checking Between Specific Body Pairs
将机械臂刚体树模型安装到无人机浮动基座上，并对安装机械臂的无人机进行可视化。
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)
运动规划问题
运动规划问题是找到从起始关节配置到期望目标配置的无碰撞路径。为此，必须定义问题的以下方面：
- 启动联合配置
- 目标关节配置
- 无人机碰撞模型
- 环境中的障碍
- 机器人的运动学约束
起始与目标关节构型
无人机（UAV）的关节构型表示其在世界坐标系中的位姿。因此，定义无人机的构型向量以描述其在世界坐标系中的位置为 [q_uav t_uav q_robotic_arm]，其中：
q_uav是刚体“uav”上浮动关节相对于固定基座的朝向，表示为一个 1×4 的单位四元数。t_uav是刚体“uav”上浮动关节相对于固定基座的位置，表示为一个 1×3 向量。q_robotic_arm是机械臂 openmanip 的关节构型向量，表示为一个 1×6 向量。
将起始构型设置为：浮动体“uav”位于 xyz 坐标 [0 0 -1]，且机械臂第三个关节的角度为 -π/3 弧度。
qstart = homeConfiguration(uavWithArm);
qstart(5:7) = [0 0 -1];
qstart(end-3) = -pi/3;
使用起始构型并对其进行修改以获得目标构型。将“uav”主体的目标构型设置为 xyz 坐标 [2 0 2]，并设定其朝向为 [π/2 0 0] 的 ZYX 欧拉角。
qgoal = qstart;
qgoal(5:7) = [2 0 2];
qgoal(1:4) = eul2quat([pi/2 0 0]);
在环境中添加障碍物
使用碰撞盒创建环境，并设置这些盒子的位置，使其在起始构型与目标构型之间形成一道墙。
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
机器人的运动学约束
在此示例中，设置与 “uav” rigidBody 关联的浮动关节的平移 PositionLimits ，使其被限制在工作区框架的笛卡尔边界（例如物理安全笼）内飞行，即 X 轴 [-1,3] 、Y 轴 [-2,2] 和 Z 轴 [-2.5,2.5] 。请注意， floating rigidBodyJoint 的旋转限制无法设置，并且为 NaNs 。
uavBody=getBody(uavWithArm,"uav");
uavBody.Joint.PositionLimits(5:end,:)=[-1,3;-2,2;-2.5,2.5];
寻找无碰撞路径
在起始关节配置和目标关节配置之间找到一条无碰撞路径。为了找到这样的路径，本示例使用 manipulatorRRT 来规划一条几何无碰撞路径。
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))
