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

1、干涉碰撞检查

　　构建 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并行处理。我正在思考如何利用异构并行实现碰撞检测。

2、轨迹规划

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