機器人學 —— 軌跡規划（Configuration Space）

　　之前的軌跡規划中，我們只考慮了質點，沒有考慮機器人的外形與結構。直接在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 的另一頭穿越出來。如下：