✅博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 如需沟通交流，扫描文章底部二维码。
（1）MSDT碰撞检测方法：
基于Minkowski Sum与对偶三角形。将工业机器人的每个连杆简化为三角形或四边形，取三个顶点形成主三角形。然后根据连杆轴线构建对偶三角形，对偶三角形由背离原三角形的法向偏移得到。将两个对偶三角形向障碍物（球体或长方体）进行Minkowski Sum膨胀，使得机器人与障碍物间的碰撞检测转化为膨胀体间的距离计算。具体而言，对于球体障碍物，膨胀体为三角形偏移半径；对于另一台机器人的连杆包络长方体，膨胀后为三角形与长方体的Minkowski和。碰撞条件简化为检查膨胀三角形是否包含原点或线段相交。在KUKA和ABB双机器人场景中，节点检测耗时从传统网格方法的每次0.12秒降至0.03秒，效率提升75%。
（2）非线性惯性权重粒子群算法NIW-PSO路径规划：
在关节空间中离散化6个关节的角度序列构成粒子，粒子维度为6×T，T为路径分段数。适应度函数融合路径总长度、与障碍物的MSDT碰撞惩罚以及关节速度变化率平滑项。将惯性权重w设为余弦函数形式w(t)=0.9-0.5(1+cos(π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(axis=0) dual_pts = pts + (pts - center) # 对偶翻倍 return dual_pts def triangle_sphere_minkowski_sum(tri, sphere_radius): # 膨胀为检查三角形每个边偏移球半径 expanded = [] for i in range(3): edge = tri[(i+1)%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 (d1>0 and d2>0 and d3>0) or (d1<0 and d2<0 and d3<0) for i in range(3): if point_in_triangle(obs_sphere_center, expanded_tri[i], expanded_tri[(i+1)%3], tri[i]): return True return False # NIW-PSO算法 def niw_pso(cost_func, dim, bounds, max_iter=100, pop_size=50): 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如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
