✅博主简介擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导毕业论文、期刊论文经验交流。✅ 如需沟通交流扫描文章底部二维码。1MSDT碰撞检测方法基于Minkowski Sum与对偶三角形。将工业机器人的每个连杆简化为三角形或四边形取三个顶点形成主三角形。然后根据连杆轴线构建对偶三角形对偶三角形由背离原三角形的法向偏移得到。将两个对偶三角形向障碍物球体或长方体进行Minkowski Sum膨胀使得机器人与障碍物间的碰撞检测转化为膨胀体间的距离计算。具体而言对于球体障碍物膨胀体为三角形偏移半径对于另一台机器人的连杆包络长方体膨胀后为三角形与长方体的Minkowski和。碰撞条件简化为检查膨胀三角形是否包含原点或线段相交。在KUKA和ABB双机器人场景中节点检测耗时从传统网格方法的每次0.12秒降至0.03秒效率提升75%。2非线性惯性权重粒子群算法NIW-PSO路径规划在关节空间中离散化6个关节的角度序列构成粒子粒子维度为6×TT为路径分段数。适应度函数融合路径总长度、与障碍物的MSDT碰撞惩罚以及关节速度变化率平滑项。将惯性权重w设为余弦函数形式w(t)0.9-0.5(1cos(πt/Tmax))初期权重较大以利于全局搜索后期权重较小以加速收敛。每次迭代中对粒子进行MSDT快速碰撞检测若碰撞则施加高惩罚值。在双机器人齿轮轴搬运任务中NIW-PSO在70代内收敛到无碰撞路径而传统PSO需120代。规划路径成功实现双机从抓取点到放置点的无碰撞协调运动。3紧约束闭环运动链协调与ROS实验验证针对双机器人共同搬运同一工件的紧约束条件建立主从运动学约束方程主机器人运动轨迹由任务指定从机器人通过闭环链关系实时解算。利用MSDT检测双机之间可能的碰撞在迭代过程中排查。基于ROS开发双机器人路径规划软件提供交互式任务设置界面。在真实KUKA KR6和ABB IRB120实验平台上设计了从料仓取齿轮轴并搬运至组装台的示范场景NIW-PSO规划的无碰撞路径使双机总运行时间较手动示教缩短27%。import numpy as np from scipy.spatial import distance # 对偶三角形与Minkowski和碰撞检测 def dual_triangle(pts): # pts: 3x3 三角形顶点 center pts.mean(axis0) dual_pts pts (pts - center) # 对偶翻倍 return dual_pts def triangle_sphere_minkowski_sum(tri, sphere_radius): # 膨胀为检查三角形每个边偏移球半径 expanded [] for i in range(3): edge tri[(i1)%3] - tri[i] normal np.array([-edge[1], edge[0], 0]) # 2D情况 normal normal / np.linalg.norm(normal) * sphere_radius expanded.append(tri[i] normal) return np.array(expanded) def check_collision(tri, obs_sphere_center, radius): # 基于MSDT检测碰撞 expanded_tri triangle_sphere_minkowski_sum(tri, radius) # 检查原点是否在膨胀三角形内平移至obs_sphere_center # 采用重心坐标法 def point_in_triangle(pt, v1, v2, v3): d1 (pt[0]-v2[0])*(v1[1]-v2[1]) - (pt[1]-v2[1])*(v1[0]-v2[0]) d2 (pt[0]-v3[0])*(v2[1]-v3[1]) - (pt[1]-v3[1])*(v2[0]-v3[0]) d3 (pt[0]-v1[0])*(v3[1]-v1[1]) - (pt[1]-v1[1])*(v3[0]-v1[0]) return (d10 and d20 and d30) or (d10 and d20 and d30) for i in range(3): if point_in_triangle(obs_sphere_center, expanded_tri[i], expanded_tri[(i1)%3], tri[i]): return True return False # NIW-PSO算法 def niw_pso(cost_func, dim, bounds, max_iter100, pop_size50): w_max, w_min 0.9, 0.4 c1, c2 2.0, 2.0 pos np.random.uniform(bounds[:,0], bounds[:,1], (pop_size, dim)) vel np.random.uniform(-1,1, (pop_size, dim)) pbest pos.copy() pbest_cost np.array([cost_func(p) for p in pos]) gbest pbest[np.argmin(pbest_cost)] gbest_cost min(pbest_cost) for t in range(max_iter): w w_max - (w_max - w_min) * (1 np.cos(np.pi * t / max_iter)) / 2.0 for i in range(pop_size): r1, r2 np.random.rand(dim), np.random.rand(dim) vel[i] w*vel[i] c1*r1*(pbest[i]-pos[i]) c2*r2*(gbest-pos[i]) pos[i] vel[i] pos[i] np.clip(pos[i], bounds[:,0], bounds[:,1]) cost cost_func(pos[i]) if cost pbest_cost[i]: pbest[i] pos[i]; pbest_cost[i] cost if cost gbest_cost: gbest pos[i]; gbest_cost cost return gbest, gbest_cost # 示例适应度函数含碰撞惩罚 def fitness_dual_robot(x): # 解码为关节序列、正运动学计算连杆位置、调用MSDT collision check_collision(np.random.rand(3,2), np.array([0.5,0.5]), 0.1) penalty 1000 if collision else 0 return np.linalg.norm(x) penalty如有问题可以直接沟通