机器人学 —— 轨迹规划（Configuration Space）

　　之前的轨迹规划中，我们只考虑了质点，没有考虑机器人的外形与结构。直接在obstacle map 中进行轨迹规划，然而世纪情况中，机器人有固定外形，可能会和障碍物发生碰撞。此情况下，我们针对机器人自由度进行建模，给定其运动空间，如果是扫地机器人，那么其自由度是x-y的平移，如果是N自由度机械臂，其自由度是电机转角，我们针对此自由度，构建Configuration Space 并在其中使用A* 或者DJ 算法进行轨迹规划。

　　

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;

View Code
　　此算法的缺点是非常非常慢。而且由于需要使用流进行判断，不方便GPU并行处理。我正在思考如何利用异构并行实现碰撞检测。

2、轨迹规划

　　对于机械臂而言，轨迹规划算法与平面机器人差异并不大，但是需要注意的是，机械臂的关节角可以认为是360度的。具体体现在可以从Configuration Space 的另一头穿越出来。如下：