标签：
之前的轨迹规划中，我们只考虑了质点，没有考虑机器人的外形与结构。直接在obstacle map 中进行轨迹规划，然而世纪情况中，机器人有固定外形，可能会和障碍物发生碰撞。此情况下，我们针对机器人自由度进行建模，给定其运动空间，如果是扫地机器人，那么其自由度是x-y的平移，如果是N自由度机械臂，其自由度是电机转角，我们针对此自由度，构建Configuration Space 并在其中使用A* 或者DJ 算法进行轨迹规划。
构建 Configuration Space 任务的pipeline 如下：空间离散化 --- 干涉碰撞检查 --- 生成 Obstacle Map --- Motion Plan.
所以，其核心的核心就是干涉碰撞检查，检查 Configuration Space 中的点，会不会导致机器人与障碍物碰撞。 干涉碰撞检测的方法是将机器人与物体进行三角化，并利用三角重叠检测的方法来对物体碰撞进行检查。
两个三角形共有6条边，如果存在某条边，使得顶点分别位于边的两边则两个三角形未发生碰撞。代码如下：
P{1} = P1;
P{2} = P2;
linesP1(1,:) = cross([P1(2,:) 1],[P1(3,:) 1]);
linesP1(2,:) = cross([P1(1,:) 1],[P1(3,:) 1]);
linesP1(3,:) = cross([P1(1,:) 1],[P1(2,:) 1]);
Lines{1} = linesP1;
linesP2(1,:) = cross([P2(2,:) 1],[P2(3,:) 1]);
linesP2(2,:) = cross([P2(1,:) 1],[P2(3,:) 1]);
linesP2(3,:) = cross([P2(1,:) 1],[P2(2,:) 1]);
Lines{2} = linesP2;
Result_Judge = zeros(3,1);
lines = Lines{1};
for line_idx = 1:3
line = lines(line_idx,:);
Point_Tri = [P{1}(line_idx,:) 1]*line‘;
for point_idx = 1:3
Result_Judge(point_idx) = [P{2}(point_idx,:) 1]*line‘;
end
if Point_Tri>0 && all(Result_Judge<0)
flag = false;
return
elseif Point_Tri<0 && all(Result_Judge>0)
flag = false;
return
end
end
lines = Lines{2};
for line_idx = 1:3
line = lines(line_idx,:);
Point_Tri = [P{2}(line_idx,:) 1]*line‘;
for point_idx = 1:3
Result_Judge(point_idx) = [P{1}(point_idx,:) 1]*line‘;
end
if Point_Tri>0 && all(Result_Judge<0)
flag = false;
return
elseif Point_Tri<0 && all(Result_Judge>0)
flag = false;
return
end
end
flag = true;
此算法的缺点是非常非常慢。而且由于需要使用流进行判断，不方便GPU并行处理。我正在思考如何利用异构并行实现碰撞检测。
对于机械臂而言，轨迹规划算法与平面机器人差异并不大，但是需要注意的是，机械臂的关节角可以认为是360度的。具体体现在可以从Configuration Space 的另一头穿越出来。如下：
机器人学 —— 轨迹规划（Configuration Space）
标签：
原文地址：http://www.cnblogs.com/ironstark/p/5537270.html
