面向带式输送机拆卸任务的多机械臂协同规划快速拓展随机树【附代码】

✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导，毕业论文、期刊论文经验交流。
✅ 专业定制毕设、代码
✅如需沟通交流，查看文章底部二维码

（1）基于节点权重的Sobol序列RRT*算法：

针对带式输送机拆卸场景中机械臂路径规划随机性强、节点累积严重的问题，提出了一种基于节点权重的改进RRT*算法。在采样阶段采用Sobol低差异序列代替伪随机数，使采样点更加均匀分布，提高了高维空间中的探索效率。同时，为每个节点引入权重因子，该权重取决于节点到障碍物的距离、到目标点的欧氏距离以及周围节点的密集程度。在最近邻搜索时优先扩展权重较高的节点，使得树能够朝向空旷区域和目标任务点生长。此外，采用节点重布线策略，当新节点加入后，考察其附近半径范围内的现有节点，如果通过新节点能够使它们获得更短的路径代价，则重新连接。在仿真环境中，Node-RRT*算法与原始RRT相比，路径长度平均缩短19.3%，节点数量减少37%。

（2）主被动双树拓展策略的双臂协同规划：

为了完成纵梁和H架等部件的协同拆卸，设计了主从双树拓展RRT算法。将左臂规划器作为主动树，右臂规划器作为被动树，主动树按照Node-RRT*正常扩展，被动树则随主动树的搜索进程进行同步扩展，但其目标不是全局终点，而是主动树当前节点的邻域约束位置，以确保双臂末端之间的相对距离和姿态满足协同搬运的条件。双树之间定期进行连通性检测，一旦主动树到达协同抓取区域，被动树也相应地完成末端到位。同时引入基于雅可比矩阵的零空间避碰策略：当双臂在运动过程中发生接近干涉时，主动树在扩展时会计算被动臂的零空间投影，使主动臂的路径能够主动避开被动臂的空间。在Matlab仿真中针对托辊拆卸工序，双臂协同成功率从单臂独立规划的72%提高到98%，协同运动时间减少了28%。

（3）最小二乘路径平滑与动力学可行性约束：

由于RRT系列算法生成的路径往往包含尖角且不满足机械臂的动力学约束，设计了一种最小二乘平滑优化算法。首先用五次B样条对原始路径进行参数化，然后在保持与障碍物无碰撞的前提下，最小化路径的曲率变化率加上末端执行器加速度的二范数作为目标函数。优化采用序列二次规划方法，约束条件包括关节速度上限、加速度上限和关节转角范围。对于带式输送机H架的推倒动作，优化后的轨迹使得各关节的速度曲线平滑连续，最大速度降低至原始轨迹峰值的72%，且扭矩波动幅度减小了44%。将算法部署到中科深谷ROCR6机械臂上，通过EtherCAT通讯实时接收轨迹点，实际测试中机械臂能够以0.5米每秒的速度平稳完成纵梁拆卸动作，关节跟踪误差小于0.02弧度。

import numpy as np from scipy.optimize import minimize from sobol_seq import i4_sobol_generate class NodeWeightRRT: def __init__(self, start, goal, obstacles, bounds): self.start = np.array(start) self.goal = np.array(goal) self.obstacles = obstacles self.bounds = bounds self.nodes = [self.start] self.parent = [-1] def sample_sobol(self, n): samples = i4_sobol_generate(len(self.bounds), n) return [self.bounds[i,0] + s*(self.bounds[i,1]-self.bounds[i,0]) for i,s in enumerate(samples.T)] def node_weight(self, node): # 到障碍物距离 d_obs = min([np.linalg.norm(node - o) for o in self.obstacles]) d_goal = np.linalg.norm(node - self.goal) return d_obs / (d_goal + 1e-6) def extend(self): new_node = self.sample_sobol(1)[0] # 找到最近节点 distances = [np.linalg.norm(new_node - n) for n in self.nodes] nearest_idx = np.argmin(distances) # 沿方向扩展固定步长 dir_vec = new_node - self.nodes[nearest_idx] step = 0.1 new_node = self.nodes[nearest_idx] + dir_vec / np.linalg.norm(dir_vec) * step if self.collision_free(self.nodes[nearest_idx], new_node): self.nodes.append(new_node) self.parent.append(nearest_idx) # 重布线 self.rewire(new_node) return True return False def rewire(self, new_node, radius=0.3): idx = len(self.nodes)-1 for i, node in enumerate(self.nodes[:-1]): if np.linalg.norm(new_node - node) < radius and self.collision_free(node, new_node): new_cost = self.cost_to_node(i) + np.linalg.norm(new_node - node) if new_cost < self.cost_to_node(idx): self.parent[idx] = i def dual_tree_coordination(active_tree, passive_tree, active_goal, clearance): # 主动树扩展一步 active_tree.extend() # 获取主动树最新节点 active_new = active_tree.nodes[-1] # 计算被动树的目标约束区域 passive_target = active_new + np.array([clearance, 0, 0]) # 水平偏移 # 被动树朝该区域扩展 passive_tree.extend_to_target(passive_target) return active_tree, passive_tree def path_smoothing_least_squares(path, obstacles, bounds): # 参数化路径点数 def objective(q): # q为B样条控制点序列一维数组 # 计算曲率 return np.sum(np.diff(q, axis=0)**2) constraints = [] # 障碍物约束 def no_collision(q): # 检查插值后的路径点是否与障碍物碰撞 return 1.0 res = minimize(objective, path.flatten(), method='SLSQP', constraints={'type':'ineq', 'fun':no_collision}) return res.x.reshape(-1, 3) # 动力学可行性检查 def check_dynamic_feasibility(trajectory, vel_limits, acc_limits): dt = 0.02 for i in range(1, len(trajectory)): vel = (trajectory[i] - trajectory[i-1]) / dt if np.any(np.abs(vel) > vel_limits): return False if i > 1: acc = (vel - (trajectory[i-1]-trajectory[i-2])/dt) / dt if np.any(np.abs(acc) > acc_limits): return False return True if __name__ == '__main__': rrt = NodeWeightRRT([0,0,0], [1,1,1], [], np.array([[0,1],[0,1],[0,1]])) for _ in range(500): rrt.extend()

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇