5_位形空间的示例_宾夕法尼亚大学机器人运动规划专项课程【学习笔记（附Assignment:Configuration Space(Part 1 of 2 & Part 2 of 2)代码）】_piano mover鈥檚 problem

1. RR arm

这是个简易的二维机械臂，有两个旋转关节；像以前那样。我们可以理清这个机器人所有可能的运动轨迹，并将它们与转动角数组[θ1,θ2]关联起来，在这个案例，两个角可以自由地从0度转动到360度。

下图动画展示的是连杆二维机械臂是如何运动的以及它在位形空间中相应的轨迹。当该机器人持续运功，在位形空间中，与它对应的红点也会移动（红点），红点从图形的一侧消失，然后从另一侧出现，这是旋转运动造成的，即当θ1等于0°或360°时，对应的机器人位形空间点相同，θ2也一样。

引入障碍物，思考为了避免与障碍物发生碰撞，哪些位形空间是无法到达的；
下图显示的是机器人、障碍物、以及他们在位形空间中对应的情况。

引入障碍物，可以发现，实际上，二维平面中的简单多边形障碍物的形状，在位形空间中会变得非常有趣；
再次强调路径规划的实质是在位形空间内，引导机器人从一个点前往另一个点。

上图是个运动轨迹的样例，在位形空间内，机器人从一个位置移动到另个位置；这里会体现出位形空间的拓扑性质，为了避免位形空间中的障碍墙（图的中间部分），机器人从底部消失，然后在顶部再次出现，最终抵达位形空间的终点。

2. Piano Mover’s Problem

引入个更加复杂的例子，平移+旋转机器人，这意味着其自由度增加到了3，在最初的平移(横纵两个维度)的基础上引入(三维的)旋转自由度
我们可以用 [tx,ty,theta] 这个数组来表示机器人的位置，其中和y依然代表平面内对应参考点的位置，theta 则代表旋转角度。

当我们再次将障碍物引入到活动空间时，可以认为位形空间的集是有限的

由于机器人有三个维度，所以位形空间可以被看成一个三维空间

动画展示的是位形空间中的障碍表面，这个障碍还是前一张图中的障碍。

垂直方向(z轴)表示旋转角度theta，而另外两个轴(x，y轴) ，表示平移参数tx和ty，注意在这个图像中。这些可视化的表面相当于位形空间中障碍的表面。

如前所述，运动规划是规划出起点和终点之间的路径，而不触碰位形空间内的障碍

下图所示动画展示了机器人避开障碍物，穿越空间的过程

在下面动画中，我们将机器人穿过位形空间的轨迹简化成条红线，注意这条红线从起点移动到终点的过程中是如何在位形障碍附近或中间蜿蜒穿梭而无需穿越障碍表面的。

最重要的是要理解位形这个概念，它跟机器人的设定是相关的，还要想清楚哪些位形空间可通行，哪些不可通行；
事实上，把机器人在位形空间中的运动轨迹看成某个点(的运动轨迹)是很正常的。

3. 其它参考学习资源

感谢 B 站 up 主 Here_Kin 的翻译和分享！
【自制中英】宾夕法尼亚大学机器人专项课程二 运动规划 - Robotics:Motion and Planning
感谢博主 Kin_Zhang 的总结与分享！
2 Robotics: Computational Motion Planning 第2+3+4周 课后习题解答

4. Assignment:Configuration Space(Part 1 of 2)

下面是旁听学员能看到的关于编程作业的描述（也太少惹）：

您将编写一个程序来帮助引导双连杆机械臂从一种配置转换为另一种配置，同时避开工作区中的障碍物。作业分为两部分，每一部分都提供了一些处理配置空间表示的机会。

下面是有权限学员所看到的编程作业说明（交了钱就是不一样）：

参见博客2 Robotics: Computational Motion Planning 第2+3+4周 课后习题解答 中 Configuration Space: Part 1 of 2 部分。

INTRODUCTION
Configuration Space 编程作业的主要目标是为学生提供在现实世界环境中实际应用配置空间的经验。在此编程作业中，您将编写一个程序来帮助引导双连杆机械臂从一个配置空间到另一个配置空间，同时避开工作区中的障碍物。机器人的配置由两个关节角度 θ1 和 θ2 捕获，如图 1 所示。这些角度用 0 到 360 度之间的值表示。

在图 2 中，左图显示了描绘机器人和工作区障碍物的图形，右图显示了相应配置空间（白色）和配置空间障碍物（黑色）的图。横轴和纵轴分别对应于 θ1 和 θ2。在左图中，机器人和障碍物均由三角形集合表示。通过检查机器人中的任何三角形是否与障碍物中的任何三角形相交，将机器人（和障碍物）分解为三角形有助于我们确定特定的机器人配置是否会导致碰撞。

INRTODUCTION OF TRIANGLES

对于这个特定的任务，由于机械臂和障碍物被假定为三角形的集合，任何形式的现实世界碰撞都被简化为机械臂的三角形和障碍物之间的交叉点。

理解这一概念的众多方法之一是考虑所有 6 条边（每个三角形 3 条）以及它们是否充当分隔线，其中所有顶点位于一个三角形的一侧。

此外，可能需要检查的情况如下：

从左到右，可能的场景包括：

• 1、不相交的三角形
• 2、在一点相交的三角形（一条线对一点）
• 3、在一点相交的三角形（一点对一点）
• 4、通过线重叠相交的三角形
• 5、在多个点相交的三角形
• 6、一个三角形重叠另一个

FUNCTION DESCRIPTION

对于这部分作业，您必须使用以下输入和输出参数创建函数 triangle_intersection：
P1、P2：一个 3x2 数组（每个），描述三角形的顶点，其中第一列表示 x 坐标，第二列表示 y 坐标。
flag：函数的返回值，如果确定三角形之间的交集（包括重叠），则设置为true，否则设置为false。

完成后，您可以尝试多个场景来检查您的算法是否适用于上述所有情况。

4.1 三角形碰撞检测代码及测试结果

三角形碰撞检测算法采用的是博客2 Robotics: Computational Motion Planning 第2+3+4周 课后习题解答 中 Configuration Space: Part 1 of 2 部分用行列式求三角形面积的解决思路，但是该篇博主给出的代码中存在两处问题，以致于不能应对上面提出的六种所有三角形位置关系情况。

• 问题一：Area_base 的求解需要取绝对值；
• 问题二：由于 MATLAB 软件本身的保留计算精度问题，即使 Area_base 和 Area 相差甚微，却依旧不会满足 Area_base==Area 这么严格的条件，极容易造成误判。

改正后代码：
call_my_function.m

x = [0.2,0.2,0.7]; 
y = [4,2,3];
P1 = [x;y]';

x = [0,0,1];
y = [1,5,3];
P2 = [x;y]';

line([P1(:,1)' P1(1,1)],[P1(:,2)' P1(1,2)],'Color','r')
line([P2(:,1)' P2(1,1)],[P2(:,2)' P2(1,2)],'Color','b')

flag = triangle_intersection(P1,P2);
if flag == 1
    disp("发生碰撞")
else
    disp("未发生碰撞")
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

triangle_intersection.m

function flag = triangle_intersection(P1, P2)
% triangle_test : returns true if the triangles overlap and false otherwise
% P1、P2：一个 3x2 数组（每个），描述三角形的顶点，其中第一列表示 x 坐标，第二列表示 y 坐标。
% flag：函数的返回值，如果确定三角形之间的交集（包括重叠），则设置为true，否则设置为false。
% 简单来说，该函数就是用来判断两个三角形是否相交，相交返回1，不相交返回0；

% *******************************************************************
flag = Position(P1,P2);

if flag == false
    flag = Position(P2,P1);
end

% 此处函数判断 P2 三角形各个顶点是否在 P1 三角形内
function Point = Position(Po1,Po2)
    base = [Po1 ones(3,1)];
    Area_base = abs(0.5*det(base));
    Point = false;
    for i=1:3
        Area = abs(Cal_Area(Po2(i,:),Po1(1:2,:)));
        Area = Area+abs(Cal_Area(Po2(i,:),Po1(2:3,:)));
        Area = Area+abs(Cal_Area(Po2(i,:),[Po1(1,:);Po1(3,:)]));       
        if Area-Area_base<1e-10  
            Point = true; 
            break;
        end
    end
end

% 此函数用于计算三角形面积
function Area = Cal_Area(temp,Tra)
    Area = 0.5*det([[temp;Tra] ones(3,1)]);
end
% *******************************************************************
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

测试结果如下：

情况一

情况二

情况三

情况四

情况五

情况六

5. Assignment:Configuration Space(Part 2 of 2)

编程作业说明：

参见博客2 Robotics: Computational Motion Planning 第2+3+4周 课后习题解答 中 Configuration Space: Part 2 of 2 部分。

INTRODUCTION
在作业的前一部分中，您实现了一个算法来检查两个三角形是否在任何一点相交。 由于机械臂和障碍物是一组三角形，因此可以检查配置是否可行变得不可或缺。

对于第 1 周的家庭作业，您被要求完成 DijkstraGrid 函数，该函数用于通过基于简单网格的环境规划路径，使用 2D 逻辑数组建模。在作业的以下部分，您将通过完成函数 DijkstraTorus 来扩展该想法。

DijkstraTorus 将根据机械臂与工作空间中障碍物之间的三角形交点导出的配置空间，实施 Dijkstra 算法来计算从起始配置到结束配置的最短可行路线。

IMPLEMENTATION
此函数的输入和输出参数解释如下：
input map：一个逻辑数组，其中自由空间单元为 false 或 0，障碍为 true 或 1；
start_coords, dest_coords：开始和结束单元格的坐标，第一个是行，第二个是列；
route：包含沿从 start 到 dest 的最短路径的单元格的线性索引的数组，如果没有路径，则为空数组。

注意：

• 该特定的函数包含一个变量标签，它使您可以选择是否希望查看函数的输出
• 以下代码使用现有版本的 triangle_intersection.m 因为它对 DijkstraTorus 的实施至关重要。如果您希望应用您的代码版本，请将您的代码插入下面提供的 matlab zip 文件链接中提供的 triangle_intersection.m 中
• 为了简化和减少计算时间，已经加载了现有的配置空间。 在下面的 MATLAB zip 文件链接中已经提到了创建这个配置空间的代码TwoLinkCspace.m
• 如果您希望查看代码工作方式的动画，请从上一屏幕的摘要窗口下载 MATLAB zip 文件并插入您的代码。

5.1 代码及测试结果

function route = DijkstraTorus (input_map, start_coords, dest_coords)
% Run Dijkstra's algorithm on a grid.
% Inputs : 
%   input_map : 一个逻辑数组，其中自由空间单元为 false 或 0，障碍为 true 或 1；
%   start_coords and dest_coords : 开始和结束单元格的坐标，第一个是行，第二个是列；
% Output :
%   route : 包含沿从 start 到 dest 的最短路径的单元格的线性索引的数组，如果没有路径，则为空数组。

% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue  - on list
% 5 - green - start
% 6 - yellow - route
% 7 - brown - destination

cmap = [1 1 1; ...   % 白色
        0 0 0; ...   % 黑色
        1 0 0; ...   % 红色
        0 0 1; ...   % 蓝色
        0 1 0; ...   % 绿色
        1 1 0; ...   % 黄色
	0.5 0.5 0.5];    % 灰色

colormap(cmap);

[nrows, ncols] = size(input_map);

% map - a table that keeps track of the state of each grid cell
map = zeros(nrows,ncols);

map(~input_map) = 1;
map(input_map)  = 2;

% 起始节点以及目标节点的线性索引
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));

map(start_node) = 5;
map(dest_node)  = 7;

% Initialize distance array
distances = Inf(nrows,ncols);

% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);

distances(start_node) = 0;

% Main Loop
while true
    
    % Draw current map
    map(start_node) = 5;
    map(dest_node) = 7;
    
    image(1.5, 1.5, map);
    grid on;
    axis image;
    drawnow;
    
    % Find the node with the minimum distance
    [min_dist, current] = min(distances(:)); % min_dist---当前最短节点的距离   current---当前最短节点索引
    
    if ((current == dest_node) || isinf(min_dist))
        break;
    end
    
    % Update map
    map(current) = 3;         % 把当前节点标记为已访问
    distances(current) = Inf; % remove this node from further consideration
    
    % 得到当前节点的横纵坐标
    [i, j] = ind2sub(size(distances), current);
    
    % 访问当前节点的每个邻居，并适当地更新映射、距离和父表。
    % *******************************************************************
    south_node=[i,j-1];
    north_node=[i,j+1];
    east_node=[i+1,j];
    west_node=[i-1,j];
    neighbourhood=[south_node;north_node;west_node;east_node];
    if(i==1)
        neighbourhood(3,:)=[nrows, j];
    end
    if (i == nrows)
        neighbourhood(4,:) =[1, j];
    end
    if (j == 1)
        neighbourhood(1,:) = [i, ncols];
    end
    if (j == ncols)
        neighbourhood(2,:) = [i, 1];
    end
    for k=1:4
        update(neighbourhood(k,1),neighbourhood(k,2),1+min_dist,current);
    end 
    
    % *******************************************************************
end

if (isinf(distances(dest_node)))
    route = [];
else
    route = [dest_node];
    
    while (parent(route(1)) ~= 0)
        route = [parent(route(1)), route];
    end
end

    for k = 2:length(route) - 1        
        map(route(k)) = 6;
        pause(0.1);
        image(1.5, 1.5, map);
        grid on;
        axis image;
    end

    function update (i,j,d,p)
        % 如果此邻居没有被访问过、不是障碍物、不是起点 并且 如果此邻居节点距离值>当前父节点距离值+1，就更新当前相邻节点距离值；
        if ( (map(i,j) ~= 2) && (map(i,j) ~= 3) && (map(i,j) ~= 5) && (distances(i,j) > d) )
            distances(i,j) = d; % 更新距离值
            map(i,j) = 4; % 让该邻居节点进入当前探索列表中
            parent(i,j) = p; %把现在的邻居点加入到父列表中
        end
    end

end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131

测试结果如下：