✅博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 如需沟通交流，扫描文章底部二维码。
（1）UR10与AY5运动学建模与AABB碰撞检测：
采用改进DH参数法分别对UR10六轴机械臂和AY5-900七轴机械臂进行运动学建模。UR10连杆参数：d1=0.1273m, a2=-0.612m, a3=-0.5723m, d4=0.1639m, d5=0.1157m, d6=0.0922m；AY5-900因额外关节增加了a4=0.25m。利用MATLAB Robotics Toolbox建立仿真模型，通过蒙特卡洛法在关节限位内随机采样50000组构型，计算并绘制出UR10工作空间近似为半径1.6m的球体，AY5-900为1.9m。碰撞检测采用AABB层次包围盒：将两个机械臂的连杆简化为一系列长方体包围盒，每个包围盒根据连杆长度和半径动态调整尺寸，通过分离轴定理进行相交测试。仿真测试10000次随机位姿对，检测准确率100%，单次检测平均耗时0.12ms，满足实时性要求。该模型嵌入到ROS MoveIt中，用于主从机械臂协作运动时的实时碰撞规避。
（2）改进RRT路径规划与五次均匀B样条轨迹规划：
针对协作激光加工任务中主从臂需保持恒定相对位姿的要求，提出一种改进双向RRT算法（Bi-RRT*）用于关节空间避碰路径规划。算法在扩展节点时引入目标偏置因子p_bias=0.15，以一定概率直接指向目标节点；同时在距离目标节点较近区域采用动态步长，步长随距离指数衰减加快收敛。利用路径修剪消除冗余节点后，路径节点数平均减少34%。在此基础上，采用五次均匀B样条对关节路径进行插值生成轨迹，控制点间隔0.05s，确保位置、速度、加速度连续。仿真对比五次多项式和五次B样条两种插值方法，B样条的关节加速度峰峰值降低23.1%，轨迹平滑度更高，因此选择B样条作为轨迹规划算法。整个规划流程在关节空间内进行，保证避碰约束，为主从机械臂协同加工作好准备。
（3）双目视觉定位与主从协作激光加工实验：
为实现工件精准抓取，搭建了基于双目相机的视觉定位系统。使用25mm焦距工业相机，基线距离120mm，拍摄角度30度。通过MATLAB相机标定工具箱对双目系统标定，重投影误差0.18像素。采用眼在手外方式完成手眼标定，求得相机与机械臂基座间的变换矩阵。图像处理流程：采集工件图像后，先进行分段线性灰度变换增强对比度，再中值滤波（5×5窗口）去噪，利用Canny算子提取边缘，对不完整边缘采用NURBS曲线拟合补全，计算形心坐标和工件方向角。基于双目视差原理通过三维重建得到形心三维坐标，重建误差在1.5mm以内。开发了基于Python的上位机软件，控制UR10主臂抓取工件、AY5-900从臂执行激光加工。在15次抓取实验中，成功抓取14次，成功率93.3%，定位误差平均0.84mm。激光加工轨迹跟踪最大偏差0.34mm，验证了主从协作运动控制算法的正确性。
import numpy as np import roboticstoolbox as rtb from scipy.interpolate import splprep, splev # 改进DH参数法建立UR10模型 def ur10_dh(): # 标准改进DH参数表 L1 = rtb.RevoluteDH(d=0.1273, a=0, alpha=np.pi/2) L2 = rtb.RevoluteDH(d=0, a=-0.612, alpha=0) L3 = rtb.RevoluteDH(d=0, a=-0.5723, alpha=0) L4 = rtb.RevoluteDH(d=0.1639, a=0, alpha=np.pi/2) L5 = rtb.RevoluteDH(d=0.1157, a=0, alpha=-np.pi/2) L6 = rtb.RevoluteDH(d=0.0922, a=0, alpha=0) robot = rtb.DHRobot([L1, L2, L3, L4, L5, L6], name='UR10') return robot # AABB碰撞检测 def aabb_collision(boxA, boxB): return (abs(boxA[0] - boxB[0]) * 2 < (boxA[3] + boxB[3])) and \ (abs(boxA[1] - boxB[1]) * 2 < (boxA[4] + boxB[4])) and \ (abs(boxA[2] - boxB[2]) * 2 < (boxA[5] + boxB[5])) # 改进Bi-RRT*规划关节空间路径 def birrt_star(start, goal, obstacle_check, max_iter=5000, bias=0.15): nodes_start = [start]; nodes_goal = [goal] tree_start = {0: {'q':start, 'parent':None}} tree_goal = {0: {'q':goal, 'parent':None}} for it in range(max_iter): if np.random.rand() < bias: q_rand = goal else: q_rand = np.random.uniform(-np.pi, np.pi, 6) # 扩展树，连接两树（省略具体实现） # ... return path # 五次均匀B样条轨迹生成 def quintic_bspline_trajectory(waypoints, dt=0.05): tck, u = splprep(waypoints.T, s=0, k=5, t=np.linspace(0,1,waypoints.shape[0])) t_sample = np.arange(0, 1, dt) traj = np.array(splev(t_sample, tck)).T return traj # 双目视觉三维重建 def stereo_reconstruct(pt_left, pt_right, stereo_params): f, B, cx, cy = stereo_params['f'], stereo_params['B'], stereo_params['cx'], stereo_params['cy'] disparity = pt_left[0] - pt_right[0] if disparity <= 0: return None Z = f * B / disparity X = (pt_left[0] - cx) * Z / f Y = (pt_left[1] - cy) * Z / f return np.array([X, Y, Z])如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
