2023年7月4日发(作者：)

5_位形空间的⽰例_宾⼣法尼亚⼤学机器⼈运动规划专项课程【学习笔记（附Assignment。。。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 的翻译和分享！感谢博主 Kin_Zhang 的总结与分享！4. Assignment:Configuration Space(Part 1 of 2)下⾯是旁听学员能看到的关于编程作业的描述（也太少惹）：您将编写⼀个程序来帮助引导双连杆机械臂从⼀种配置转换为另⼀种配置，同时避开⼯作区中的障碍物。作业分为两部分，每⼀部分都提供了⼀些处理配置空间表⽰的机会。下⾯是有权限学员所看到的编程作业说明（交了钱就是不⼀样）：参见博客 中 Configuration Space: Part 1 of 2 部分。INTRODUCTIONConfiguration 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 三⾓形碰撞检测代码及测试结果三⾓形碰撞检测算法采⽤的是博客 中 Configuration Space: Part 1 of 2 部分⽤⾏列式求三⾓形⾯积的解决思路，但是该篇博主给出的代码中存在两处问题，以致于不能应对上⾯提出的六种所有三⾓形位置关系情况。问题⼀：Area_base 的求解需要取绝对值；问题⼆：由于 MATLAB 软件本⾝的保留计算精度问题，即使 Area_base 和 Area 相差甚微，却依旧不会满⾜ Area_base==Area 这么严格的条件，极容易造成误判。改正后代码：call_my_ = [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("未发⽣碰撞")endtriangle_ion 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 endend% 此函数⽤于计算三⾓形⾯积function Area = Cal_Area(temp,Tra) Area = 0.5*det([[temp;Tra] ones(3,1)]);end% *******************************************************************end测试结果如下：情况⼀情况⼆情况三情况四情况五情况六5. Assignment:Configuration Space(Part 2 of 2)编程作业说明：参见博客 中 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 - destinationcmap = [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 cellmap = 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 arraydistances = Inf(nrows,ncols);% For each grid cell this array holds the index of its parentparent = zeros(nrows,ncols);distances(start_node) = 0;% Main Loopwhile 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

% *******************************************************************endif (isinf(distances(dest_node))) route = []; route = [];else route = [dest_node];

while (parent(route(1)) ~= 0) route = [parent(route(1)), route]; endend 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 endend测试结果如下：