简介这份资源是面向机器人学学习者与机械臂控制方向研究者的避障路径规划仿真程序包聚焦多自由度机械臂在三维复杂环境中安全、高效地从起点运动到目标点并规避障碍这一核心问题。压缩包共4个文件约4KB以mat数据文件、m脚本和txt说明为主mat文件用于保存仿真输出与中间数据m脚本承载路径规划主程序txt则可能记录算法说明或参数设置便于直接运行与二次修改。内容涉及A*、Dijkstra、势场法等典型规划思路并兼顾关节速度、加速度等动力学约束与实时性考量。目前已有125人学习适合希望理解环境建模、算法实现与仿真验证流程的读者可借助现成脚本快速复现实验、对比不同算法效果并在此基础上调整参数、扩展障碍场景为实际机器人系统设计与控制打下基础。1. 机械臂避障路径规划仿真从算法选型到可复现的工程落地很多做机械臂的朋友第一次接触避障路径规划仿真都是被一个很具体的场景逼出来的六轴机械臂在抓取工件时工作空间里突然多了一个料框或者一根立柱原本示教好的轨迹直接撞上去轻则报警停机重则撞坏末端夹具。这时候你需要的不是重新示教而是一套能在仿真里先跑通、再下发到实机的避障路径规划方案。这个方向的核心是把路径规划算法、机械臂运动学模型和碰撞检测三者放进同一个仿真环境里闭环验证。适合谁适合已经能跑通 ROS 机械臂开发基础流程、手上有 URDF 模型、想从「能动能抓」进阶到「能绕开障碍物动」的从业者。仿真不是炫技它是你上线前唯一的后悔药——真机上撞一次的成本够你在仿真里跑一万次。2. 避障路径规划到底在规划什么从构型空间说起2.1 工作空间避障和构型空间避障是两回事新手最容易混淆的一点机械臂避障不是简单地在三维空间里画一条绕开障碍物的曲线。机械臂有六个关节每个关节的角度组合构成一个六维的构型空间Configuration Space简称 C-space。障碍物在工作空间里是一个立方体但映射到构型空间后它可能变成一团形状极不规则的区域。路径规划真正要做的是在这个六维空间里找到一条从起点构型到终点构型、且不穿过障碍物映射区域的连续路径。为什么强调这个因为如果你只在笛卡尔空间做直线插补末端执行器确实走的是直线但中间关节可能已经扫过了障碍物。我见过太多案例末端轨迹看起来完美绕开了结果第三关节把旁边的传感器支架撞飞了。所以避障规划的第一原则碰撞检测必须覆盖整条机械臂的所有连杆而不是只检查末端。常见做法是用 FCLFlexible Collision Library或者 Bullet 做碰撞检测把机械臂的每个连杆用包围盒或凸包近似障碍物也用同样方式表示。检测的是连杆与障碍物之间的最小距离只要这个距离小于安全阈值就判定为碰撞。2.2 采样规划、优化规划和势场法怎么选目前主流的避障路径规划算法分三大类选型直接决定你后面调参的难度和仿真能不能收敛。第一类是采样规划代表是 RRTRapidly-exploring Random Tree和它的变种 RRT-Connect、RRT*。思路是在构型空间里随机撒点逐步扩展一棵树直到连到目标点。优点是概率完备只要路径存在采样足够多总能找到缺点也明显路径往往歪歪扭扭不光滑而且每次跑结果都不一样仿真复现性差。适合高维空间和复杂障碍物场景。第二类是优化规划代表是 CHOMP、STOMP、TrajOpt。思路是先给一条初始轨迹然后通过优化目标函数平滑度、避障距离、关节限位迭代调整。优点是路径平滑、可加约束缺点是对初始轨迹敏感障碍物多的时候容易陷局部最优。适合对轨迹质量要求高的抓取场景。第三类是人工势场法思路简单目标点产生引力障碍物产生斥力合力方向就是运动方向。优点是计算快、实时性好缺点是经典死锁问题——引力斥力抵消机械臂卡在障碍物前面不动。适合动态避障小车路径规划那类场景机械臂上一般只做局部避障补充。我一般会这样组合全局用 RRT-Connect 快速找到一条可行路径再用 TrajOpt 做后处理优化把路径拉平滑并加大避障余量。这样既有采样法的完备性又有优化法的轨迹质量。2.3 用 MoveIt 在仿真里跑通第一条避障轨迹理论说再多不如跑一遍。下面是在 ROS 环境下用 MoveIt 做避障规划的最小流程。假设你已经有一个机械臂的 URDF 或者 xacro 文件并且已经配置好了 MoveIt Setup Assistant 生成的配置包。# 启动 MoveIt 演示环境加载你的机械臂配置 roslaunch your_robot_moveit_config demo.launch # 另开终端启动 RViz 里的 Motion Planning 面板 # 在 RViz 中添加 MotionPlanning 显示插件启动后在 RViz 里你会看到机械臂模型和规划场景。接下来添加障碍物# add_obstacle.py # 在规划场景中添加一个立方体障碍物 import rospy from moveit_commander import PlanningSceneInterface from geometry_msgs.msg import PoseStamped rospy.init_node(add_obstacle) scene PlanningSceneInterface() # 等待场景同步 rospy.sleep(2) # 定义障碍物位姿 box_pose PoseStamped() box_pose.header.frame_id base_link box_pose.pose.position.x 0.4 box_pose.pose.position.y 0.0 box_pose.pose.position.z 0.3 box_pose.pose.orientation.w 1.0 # 添加立方体尺寸 0.1m x 0.1m x 0.3m scene.add_box(obstacle_box, box_pose, size(0.1, 0.1, 0.3)) rospy.sleep(1)这段代码的逻辑先初始化 PlanningSceneInterface它负责和 MoveIt 的规划场景监控器通信。然后构造一个 PoseStamped指定障碍物在 base_link 坐标系下的位置。最后调用 add_box 添加一个指定尺寸的立方体。参数说明size 是长宽高单位米frame_id 必须和你的机械臂基座坐标系一致否则障碍物会飘到错误位置。添加完障碍物后在 RViz 的 Motion Planning 面板里设置一个目标位姿点击 Plan 按钮。如果规划成功你会看到一条绕开立方体的轨迹。如果失败RViz 会提示规划失败这时候需要检查障碍物是否真的挡住了路径以及规划器的参数配置。2.4 规划器参数怎么调才不翻车MoveIt 默认用的是 OMPL 规划库里面集成了 RRT、RRT-Connect、PRM 等多种规划器。在 ompl_planning.yaml 里可以配置每个规划器的参数。以下是我常用的几个关键参数参数名作用推荐值说明longest_valid_segment_fraction碰撞检测步长0.01越小检测越精细但规划越慢goal_bias目标偏向概率0.05适当提高能加快收敛太高会降低探索性rangeRRT 扩展步长0.00 表示自动计算一般不用改timeout单次规划超时5.0秒复杂场景可加到 10attempts规划尝试次数10每次失败后重新采样这些参数没有万能值需要根据你的机械臂自由度和障碍物复杂度调整。我的血泪经验是longest_valid_segment_fraction 设大了碰撞检测会漏掉薄壁障碍物设小了规划时间成倍增加。一般从 0.01 开始试如果规划太慢再放宽到 0.02。3. 仿真环境搭建URDF、控制器和传感器怎么配3.1 URDF 模型里避坑的三个细节URDF 是机械臂仿真的地基地基没打好后面避障规划全是玄学。第一个细节collision 标签和 visual 标签要分开。visual 用于显示可以用精细的 STL 网格collision 用于碰撞检测必须用简化几何体圆柱、球、包围盒否则碰撞检测计算量大到仿真跑不动。我一般会把 collision 几何体做得比 visual 略大一圈留出安全余量。第二个细节关节限位一定要写准。避障规划是在关节限位约束下找路径如果 URDF 里限位写错了规划器可能给出一个物理上根本达不到的解。比如 UR5 的肩关节限位是 ±180 度你写成 ±90 度那机械臂后半圈的工作空间直接没了。第三个细节惯性矩阵不能全填零。Gazebo 仿真里如果惯性矩阵是零机械臂会因为物理引擎除零而乱飞这就是很多人遇到的 mujoco 加载机械臂乱动或者 gazebo 里机械臂抽搐的原因。每个连杆的质量和惯性张量要按实际值填实在没有就用 CAD 软件算。!-- URDF 片段一个带碰撞体和惯性的连杆 -- link namelink_2 visual geometry mesh filenamepackage://your_robot/meshes/link_2.stl/ /geometry /visual collision geometry cylinder length0.2 radius0.04/ /geometry origin xyz0 0 0.1 rpy0 0 0/ /collision inertial mass value1.5/ inertia ixx0.002 ixy0 ixz0 iyy0.002 iyz0 izz0.0005/ /inertial /link这段 URDF 里visual 用 STL 网格保证显示效果collision 用圆柱体简化计算inertial 填了质量和惯性张量。注意 collision 的 origin 要把圆柱体偏移到连杆的几何中心否则碰撞体会和视觉模型错位。3.2 控制器配置让仿真机械臂真的动起来URDF 只描述了几何和运动学要让机械臂在仿真里动还需要配置控制器。ROS 里常用 ros_control 框架配合 Gazebo 的 gazebo_ros_control 插件。配置文件一般放在 config 目录下# config/arm_controllers.yaml arm_controller: type: position_controllers/JointTrajectoryController joints: - shoulder_pan_joint - shoulder_lift_joint - elbow_joint - wrist_1_joint - wrist_2_joint - wrist_3_joint constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05这个配置定义了一个 JointTrajectoryController它接收 MoveIt 规划出的轨迹消息然后按时间戳逐个发送关节位置指令。goal_time 是轨迹执行完成后的稳定时间stopped_velocity_tolerance 是判定停止的速度阈值。这两个参数如果设得太小控制器会频繁报「轨迹未到达目标」其实是仿真时间步长和真实时间不一致导致的。启动顺序很重要先启动 Gazebo 加载模型再启动 ros_control 的 controller_manager最后启动 MoveIt。顺序错了会出现控制器找不到关节或者规划组没有控制器可用的情况。3.3 在仿真里加一个深度相机做动态避障静态障碍物用 PlanningSceneInterface 手动加就够了但如果你想做动态避障——比如传送带上过来的工件——就需要传感器实时更新障碍物信息。常见做法是在 URDF 里加一个深度相机Gazebo 里用 openni 插件模拟点云输出。!-- 在 URDF 里添加深度相机 -- gazebo referencecamera_link sensor typedepth namecamera update_rate10/update_rate camera horizontal_fov1.047/horizontal_fov image width640/width height480/height /image clip near0.1/near far5.0/far /clip /camera plugin namecamera_controller filenamelibgazebo_ros_openni_kinect.so baseline0.2/baseline alwaysOntrue/alwaysOn updateRate10.0/updateRate cameraNamecamera/cameraName imageTopicNamergb/image_raw/imageTopicName depthImageTopicNamedepth/image_raw/depthImageTopicName pointCloudTopicNamedepth/points/pointCloudTopicName frameNamecamera_link/frameName pointCloudCutoff0.1/pointCloudCutoff pointCloudCutoffMax5.0/pointCloudCutoffMax /plugin /sensor /gazebo这段配置在 camera_link 上挂了一个深度相机输出 640x480 的深度图和点云更新频率 10Hz。pointCloudCutoff 和 pointCloudCutoffMax 限制了点云的有效距离太近和太远的点会被过滤掉。点云话题发布后可以用 Octomap 转成八叉树地图再喂给 MoveIt 的 PlanningSceneMonitor实现动态障碍物更新。注意深度相机的点云噪声在仿真里比真实传感器小得多所以仿真里调好的避障阈值上真机后要适当放宽。我一般会在仿真阈值基础上加 2 到 3 厘米的余量。4. 避坑与排查避障规划仿真里最常见的五个翻车现场4.1 规划成功但执行时撞上障碍物现象RViz 里规划出一条完美绕开障碍物的轨迹点击 Execute 后机械臂在 Gazebo 里还是撞了。原因规划时用的碰撞检测模型和执行时 Gazebo 的物理碰撞模型不一致。MoveIt 用的是 URDF 里的 collision 几何体Gazebo 用的是它自己解析的碰撞体两者可能有细微差别。另外规划轨迹是离散的路径点控制器插补时可能走出规划器没检查过的中间构型。解决第一确保 URDF 的 collision 几何体比 visual 略大第二在 MoveIt 的 planning pipeline 里开启轨迹后处理增加路径点密度第三Gazebo 里把障碍物的 collision 也按 URDF 里的尺寸设置不要用默认值。4.2 RRT 规划时间过长甚至超时现象点击 Plan 后 RViz 卡住等十几秒才提示失败或者一直转圈。原因构型空间维度太高六轴以上障碍物映射区域太大随机采样很难碰到可行解。或者 longest_valid_segment_fraction 设得太小每次碰撞检测都极慢。解决换 RRT-Connect 或者 BiTRRT这两种双向扩展的规划器收敛更快适当增大 longest_valid_segment_fraction 到 0.02如果障碍物是薄板类考虑用 PRM 先建路图再查询。另外把 timeout 设成 5 秒失败后自动重试不要让它无限跑。4.3 仿真里机械臂抖动或者飞出去现象Gazebo 启动后机械臂原地抖动或者直接飞向空中。原因九成是 URDF 的惯性矩阵没填对。质量为零、惯性张量为零、或者惯性张量不满足三角不等式都会导致物理引擎计算出发散的加速度。另一个可能是关节的 damping 和 friction 参数没设导致控制器输出和物理响应不匹配。解决用 CAD 软件导出每个连杆的质量和惯性张量确保惯性矩阵正定。在 URDF 的 joint 标签里加 damping 和 frictionjoint nameelbow_joint typerevolute dynamics damping0.1 friction0.05/ /jointdamping 是粘性阻尼friction 是库仑摩擦这两个值能显著抑制仿真抖动。一般从 0.1 和 0.05 开始试。4.4 动态障碍物更新后规划器不响应现象深度相机点云已经发布Octomap 也更新了但 MoveIt 规划时还是按旧地图走。原因PlanningSceneMonitor 没有订阅 Octomap 的更新话题或者订阅了但没触发场景更新。MoveIt 的规划场景更新是异步的有时候需要手动调用触发。解决检查 PlanningSceneMonitor 的配置确保 octomap_monitor 的 topic 和你的点云话题一致。在代码里可以手动触发场景更新from moveit_commander import PlanningSceneInterface scene PlanningSceneInterface() # 等待场景更新 rospy.sleep(1.0) # 重新获取当前场景 scene.get_known_object_names()如果还是不行检查 Octomap 的 resolution 参数太小会导致地图更新慢太大则障碍物边界粗糙。4.5 从仿真迁移到真机后避障失效现象仿真里跑得好好的避障轨迹下发到真机后要么规划失败要么执行时碰撞。原因仿真模型和真机存在运动学参数误差连杆长度、关节零位以及碰撞检测模型和真实机械臂外形不一致。真机上还有线缆、夹具等仿真里没建模的部分。解决第一做手眼标定和关节零位校准把运动学误差降到最小第二在真机碰撞模型里把线缆和夹具的包围盒加上第三仿真里规划时把安全余量加大真机上再适当缩小。我一般会在仿真里留 5 厘米余量真机上留 2 厘米。5. 进阶技巧用轨迹优化把避障路径从「能走」变成「好走」采样规划器给出的路径往往歪歪扭扭关节速度不连续执行起来一顿一顿的。这时候需要轨迹优化做后处理。我常用的是 MoveIt 的 TrajOpt 或者 CHOMP 优化器它们能在保持避障约束的前提下把路径拉平滑、降低关节加速度。具体做法是在 ompl_planning.yaml 里配置后处理优化器planner_configs: RRTConnect: type: geometric::RRTConnect range: 0.0 TrajOpt: type: geometric::TrajOpt max_iterations: 100 collision_cost_weight: 10.0 smoothness_cost_weight: 1.0collision_cost_weight 是碰撞代价权重越大越倾向于远离障碍物smoothness_cost_weight 是平滑度权重越大轨迹越平滑但可能离障碍物更近。这两个参数需要平衡我一般从 10:1 开始调如果轨迹贴障碍物太近就加大 collision_cost_weight。验证优化效果的方法在 RViz 里显示轨迹的关节速度曲线优化后的轨迹速度应该连续且没有尖峰。另一个指标是轨迹执行时间优化后通常比原始 RRT 路径短 20% 到 30%。还有一个实用技巧如果机械臂末端要抓取工件可以在优化目标里加入末端位姿约束让优化后的轨迹在接近目标时保持末端朝向稳定。这需要在 TrajOpt 里配置 cartesian_pose_constraint指定末端在轨迹末段的位姿容差。最后说一个我踩过的坑轨迹优化不是万能的如果初始路径本身穿过了一个很窄的通道优化器可能会为了平滑而把路径拉出可行域。这时候要么换更宽松的障碍物表示要么先用 RRT* 找一条渐近最优的初始路径再优化。仿真里多花十分钟调参真机上少一次撞机这笔账怎么算都值。希望帮到你。本文还有配套的精品资源点击获取