机械臂碰撞检测 八组逆解碰撞检测 机械臂避障路径规划

将末端姿态代入机械臂逆运动模型中，求出八组逆解角度，将八组逆解角度代入机械臂正运动学方程，判断机械臂与障碍物是否发生碰撞，进行碰撞检测，以实现机械臂避障路径规划。

T = [452.12 149.09 -433.07;139.942 149.09 -320.595]; %路径上各点坐标，路径点为10个，有蚁群算法得来
G = [];  %用来存储新的障碍节点，用于栅格地图的更新
N = ndims（T）;
Q = []; %可行路径上的最终各点的关节角
c = [0 0 0 0 0 0 0 0]; %不可行点判别数组  % A为路径可行判别数，可行为1，不可行为0
sum = [];
for i =1:N    %分别对各点进行碰撞检测
    t = T（i:）;
    q = transl（t）;
    TH = niyundx_change（q）;
    for m = 1:8  %根据转角最小原则进行排序，小的放前面，大的放后面
        sum（m） = abs（TH（m1））+abs（TH（m2））+abs（TH（m3））+abs（TH（m4））+abs（TH（m5））+abs（TH（m6））;
    end
    [sim index] = sort（sum）;
    THH = [TH（index（1）:）;TH（index（2）:）;TH（index（3）:）;TH（index（4）:）;TH（index（5）:）;
        TH（index（6）:）;TH（index（7）:）;TH（index（8）:）];
    for j = 1:8   %八组关节角分别进行碰撞检测，若有可行的关节角，则后面的不继续检查
         B = pengzhuangjiance（THH（j:））;
         if B == 0
                Q（i:） = THH（j:）;
                break
         else
             c（j） = 1; %把不可行的关节角相应的判别数组里的位置置1，便于最后的关节可行检测
         end
    end
    cc = 1;
    for n = 1:8
        cc = cc*c（n）;
    end
    if cc ==1  %判别i坐标处是否所有关节角都不可行
        G = [G;T（i:）]
        A = 0  %若都不可行，表示路径不可行，该点纳入障碍节点，需重新规划
        break
    else if i == N  %若可行，判断是否已经最后一个路径点
            A = 1   %若是，则路径可行，输出各点的最优关节角
            Q
            break
        end
    end
end
        
  
             
   
    

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2020-07-15 22:35  路径检测 - 副本\
     文件        1531  2020-06-22 15:39  路径检测 - 副本\lujingjiance.m
     文件        6825  2020-06-29 19:45  路径检测 - 副本\niyundx_change.m
     文件        6679  2020-06-22 15:46  路径检测 - 副本\pengzhuangjiance.m