障碍物避障开发深度相机 碰撞检测算法嵌入式落地机械臂在工作台上一通操作猛如虎结果把旁边人的手给夹了——避障这事不搞就是安全事故。一、机械臂为什么需要避障机械臂不是在实验室真空环境里工作实际部署场景有人、有工具、有物料、有其他设备。碰撞的后果不仅是任务失败——轻则工件损坏重则伤人、毁设备、停产线。所以避障不是锦上添花而是安全刚需。哪怕你的运动规划写得再漂亮只要不具备碰撞感知能力那机械臂就是一台瞎子机器。二、深度相机选型避障的第一步是看见障碍物。深度相机是当前最实用的方案。参数Realsense D435奥比中光Astra ProAzure Kinect (ToF)测距原理红外结构光双目红外结构光Time-of-Flight有效距离0.2~10m0.6~8m0.5~5.5m分辨率1280×72030fps640×48030fps640×57630fps功耗~2.5W~2W~5.9W价格~$320~$150~$400嵌入式适配有ARM驱动驱动较成熟驱动少适配难推荐嵌入式场景首选Realsense D435——Intel有官方ARM Linux驱动社区支持好预算紧张选奥比中光Astra Pro性价比高Azure Kinect功耗大、适配差嵌入式不推荐。三、点云数据获取与预处理3.1 深度图到点云转换深度相机输出的是深度图每个像素对应一个距离值需要转换为3D点云才能做碰撞检测。转换公式X (u - cx) * depth / fx Y (v - cy) * depth / fy Z depth其中 (u, v) 是像素坐标(cx, cy) 是光心(fx, fy) 是焦距depth 是该像素的深度值。这些内参从相机标定获取。3.2 降采样滤波原始点云动辄几十万个点嵌入式上根本处理不过来。必须降采样体素滤波Voxel Grid将空间划分为固定大小的体素格子每个格子内只保留一个点取中心或均值。体素大小选5mm~10mm既能保留形状又能大幅减少点数。统计滤波剔除离群点噪声对每个点计算到邻居的平均距离偏离过大的删除。降采样后点云数量从30万降到3千~5千后续碰撞检测的计算量大幅降低。四、碰撞检测方法4.1 包围盒法AABB / OBB将机械臂各连杆和障碍物用长方体包裹检测包围盒之间是否重叠。AABB轴对齐包围盒盒子边与世界坐标轴对齐计算简单两个盒子重叠只需6个不等式判断。但近似误差大长杆斜着放时盒子体积远大于实际连杆。OBB方向包围盒盒子随连杆方向旋转近似更精确但重叠检测需要15个分离轴测试计算量是AABB的2.5倍。4.2 距离场法预计算工作空间中每个点到机械臂的最短距离生成距离场Distance Field。障碍物点云直接查询距离值距离小于阈值即碰撞。精度高但距离场需要离线预计算3D距离场占内存大分辨率1cm的1m³空间就需要1百万个格子嵌入式上内存吃不消。4.3 球体近似法——嵌入式首选将机械臂的每个连杆用若干球体近似包裹障碍物点云的每个点与这些球体做距离判断。为什么选球体球体碰撞检测只需算一个距离——点到球心的距离 球半径即碰撞。没有旋转、没有方向计算量最小。实际做法一根连杆用23个球体覆盖6轴机械臂总共用1518个球体即可近似完整外形。球体半径略大于连杆实际截面半径留安全余量。五、工作空间建模将障碍物点云从相机坐标系转换到机械臂基座坐标系才能在机械臂的工作空间里做碰撞判断。转换步骤相机外参标定确定相机相对于机械臂基座的变换矩阵旋转平移点云坐标变换P_base T_cam_to_base * P_cam裁剪工作空间范围外的点机械臂臂展范围之外的点不用管六、避障策略6.1 轨迹预检测规划好一条轨迹后先不执行而是离线仿真沿轨迹逐步计算机械臂位形用碰撞检测判断每个位形是否与障碍物干涉。无碰撞 → 直接执行有碰撞 → 重新规划路径6.2 RRT / RRT* 路径搜索碰撞后怎么重新规划RRTRapidly-exploring Random Tree是高维空间路径搜索的经典算法。RRT原理从起点开始随机采样空间中的一个点找到树中离采样点最近的节点从最近节点向采样点方向扩展一小段固定步长检查新节点是否碰撞不碰撞则加入树重复直到树扩展到目标点附近RRT*是RRT的改进版每次加入新节点后检查附近节点是否能通过新节点获得更短路径重连优化最终路径更短更优。RRT适合6维关节空间搜索因为不需要对空间做网格化随机采样天然适配高维。七、嵌入式优化嵌入式上跑避障核心矛盾是计算资源有限 vs 实时性要求高。优化策略点云降采样体素滤波后点数降到3000~5000碰撞检测简化球体近似15个球 × 5000个点 75000次距离计算每次只是一个减法平方比较预计算距离表机械臂各连杆球体中心位置随关节角度变化可以预计算常用位形的球心位置表运行时查表替代实时正运动学计算多线程点云获取和碰撞检测在独立线程不阻塞控制线程八、球体近似碰撞检测C代码#includevector#includecmathstructSphere{doublex,y,z,r;// 球心坐标和半径};structPoint3D{doublex,y,z;};// 判断点是否在球内含安全距离marginboolpoint_in_sphere(constPoint3Dpt,constSpheresph,doublemargin){doubledxpt.x-sph.x;doubledypt.y-sph.y;doubledzpt.z-sph.z;doubledist_sqdx*dxdy*dydz*dz;doublethresholdsph.rmargin;returndist_sqthreshold*threshold;}// 碰撞检测障碍物点云 vs 机械臂球体模型boolcheck_collision(conststd::vectorPoint3Dobstacles,conststd::vectorSpherearm_spheres,doublesafety_margin0.02){for(constautosph:arm_spheres){for(constautopt:obstacles){if(point_in_sphere(pt,sph,safety_margin)){returntrue;// 碰撞}}}returnfalse;// 安全}// 根据关节角度计算机械臂球体模型位置std::vectorSphereupdate_arm_spheres(constdoublejoint_angles[6],constSphere arm_template[18]){std::vectorSpherespheres;for(inti0;i18;i){// 通过正运动学变换球心位置简化示意Sphere sarm_template[i];// transform_by_forward_kinematics(s, joint_angles);spheres.push_back(s);}returnspheres;}九、性能参考在RK35884核A764核A55上的实测数据参考处理环节原始数据量处理后耗时深度图获取1280×720-~5msDMA传输点云生成降采样~30万点~4000点~15ms碰撞检测球体法18球×4000点-~3ms总计--~23ms约43fps满足实时避障需求机械臂控制周期10ms碰撞检测可以2~3个周期更新一次。避障开发的核心链条深度相机感知→点云预处理→碰撞检测→路径重规划。嵌入式落地不是把PC端算法照搬而是要在精度和速度之间做取舍——球体近似法就是这个取舍的答案精度够用计算够快落地够稳。
障碍物避障开发深度相机 碰撞检测算法嵌入式落地机械臂在工作台上一通操作猛如虎结果把旁边人的手给夹了——避障这事不搞就是安全事故。一、机械臂为什么需要避障机械臂不是在实验室真空环境里工作实际部署场景有人、有工具、有物料、有其他设备。碰撞的后果不仅是任务失败——轻则工件损坏重则伤人、毁设备、停产线。所以避障不是锦上添花而是安全刚需。哪怕你的运动规划写得再漂亮只要不具备碰撞感知能力那机械臂就是一台瞎子机器。二、深度相机选型避障的第一步是看见障碍物。深度相机是当前最实用的方案。参数Realsense D435奥比中光Astra ProAzure Kinect (ToF)测距原理红外结构光双目红外结构光Time-of-Flight有效距离0.2~10m0.6~8m0.5~5.5m分辨率1280×72030fps640×48030fps640×57630fps功耗~2.5W~2W~5.9W价格~$320~$150~$400嵌入式适配有ARM驱动驱动较成熟驱动少适配难推荐嵌入式场景首选Realsense D435——Intel有官方ARM Linux驱动社区支持好预算紧张选奥比中光Astra Pro性价比高Azure Kinect功耗大、适配差嵌入式不推荐。三、点云数据获取与预处理3.1 深度图到点云转换深度相机输出的是深度图每个像素对应一个距离值需要转换为3D点云才能做碰撞检测。转换公式X (u - cx) * depth / fx Y (v - cy) * depth / fy Z depth其中 (u, v) 是像素坐标(cx, cy) 是光心(fx, fy) 是焦距depth 是该像素的深度值。这些内参从相机标定获取。3.2 降采样滤波原始点云动辄几十万个点嵌入式上根本处理不过来。必须降采样体素滤波Voxel Grid将空间划分为固定大小的体素格子每个格子内只保留一个点取中心或均值。体素大小选5mm~10mm既能保留形状又能大幅减少点数。统计滤波剔除离群点噪声对每个点计算到邻居的平均距离偏离过大的删除。降采样后点云数量从30万降到3千~5千后续碰撞检测的计算量大幅降低。四、碰撞检测方法4.1 包围盒法AABB / OBB将机械臂各连杆和障碍物用长方体包裹检测包围盒之间是否重叠。AABB轴对齐包围盒盒子边与世界坐标轴对齐计算简单两个盒子重叠只需6个不等式判断。但近似误差大长杆斜着放时盒子体积远大于实际连杆。OBB方向包围盒盒子随连杆方向旋转近似更精确但重叠检测需要15个分离轴测试计算量是AABB的2.5倍。4.2 距离场法预计算工作空间中每个点到机械臂的最短距离生成距离场Distance Field。障碍物点云直接查询距离值距离小于阈值即碰撞。精度高但距离场需要离线预计算3D距离场占内存大分辨率1cm的1m³空间就需要1百万个格子嵌入式上内存吃不消。4.3 球体近似法——嵌入式首选将机械臂的每个连杆用若干球体近似包裹障碍物点云的每个点与这些球体做距离判断。为什么选球体球体碰撞检测只需算一个距离——点到球心的距离 球半径即碰撞。没有旋转、没有方向计算量最小。实际做法一根连杆用23个球体覆盖6轴机械臂总共用1518个球体即可近似完整外形。球体半径略大于连杆实际截面半径留安全余量。五、工作空间建模将障碍物点云从相机坐标系转换到机械臂基座坐标系才能在机械臂的工作空间里做碰撞判断。转换步骤相机外参标定确定相机相对于机械臂基座的变换矩阵旋转平移点云坐标变换P_base T_cam_to_base * P_cam裁剪工作空间范围外的点机械臂臂展范围之外的点不用管六、避障策略6.1 轨迹预检测规划好一条轨迹后先不执行而是离线仿真沿轨迹逐步计算机械臂位形用碰撞检测判断每个位形是否与障碍物干涉。无碰撞 → 直接执行有碰撞 → 重新规划路径6.2 RRT / RRT* 路径搜索碰撞后怎么重新规划RRTRapidly-exploring Random Tree是高维空间路径搜索的经典算法。RRT原理从起点开始随机采样空间中的一个点找到树中离采样点最近的节点从最近节点向采样点方向扩展一小段固定步长检查新节点是否碰撞不碰撞则加入树重复直到树扩展到目标点附近RRT*是RRT的改进版每次加入新节点后检查附近节点是否能通过新节点获得更短路径重连优化最终路径更短更优。RRT适合6维关节空间搜索因为不需要对空间做网格化随机采样天然适配高维。七、嵌入式优化嵌入式上跑避障核心矛盾是计算资源有限 vs 实时性要求高。优化策略点云降采样体素滤波后点数降到3000~5000碰撞检测简化球体近似15个球 × 5000个点 75000次距离计算每次只是一个减法平方比较预计算距离表机械臂各连杆球体中心位置随关节角度变化可以预计算常用位形的球心位置表运行时查表替代实时正运动学计算多线程点云获取和碰撞检测在独立线程不阻塞控制线程八、球体近似碰撞检测C代码#includevector#includecmathstructSphere{doublex,y,z,r;// 球心坐标和半径};structPoint3D{doublex,y,z;};// 判断点是否在球内含安全距离marginboolpoint_in_sphere(constPoint3Dpt,constSpheresph,doublemargin){doubledxpt.x-sph.x;doubledypt.y-sph.y;doubledzpt.z-sph.z;doubledist_sqdx*dxdy*dydz*dz;doublethresholdsph.rmargin;returndist_sqthreshold*threshold;}// 碰撞检测障碍物点云 vs 机械臂球体模型boolcheck_collision(conststd::vectorPoint3Dobstacles,conststd::vectorSpherearm_spheres,doublesafety_margin0.02){for(constautosph:arm_spheres){for(constautopt:obstacles){if(point_in_sphere(pt,sph,safety_margin)){returntrue;// 碰撞}}}returnfalse;// 安全}// 根据关节角度计算机械臂球体模型位置std::vectorSphereupdate_arm_spheres(constdoublejoint_angles[6],constSphere arm_template[18]){std::vectorSpherespheres;for(inti0;i18;i){// 通过正运动学变换球心位置简化示意Sphere sarm_template[i];// transform_by_forward_kinematics(s, joint_angles);spheres.push_back(s);}returnspheres;}九、性能参考在RK35884核A764核A55上的实测数据参考处理环节原始数据量处理后耗时深度图获取1280×720-~5msDMA传输点云生成降采样~30万点~4000点~15ms碰撞检测球体法18球×4000点-~3ms总计--~23ms约43fps满足实时避障需求机械臂控制周期10ms碰撞检测可以2~3个周期更新一次。避障开发的核心链条深度相机感知→点云预处理→碰撞检测→路径重规划。嵌入式落地不是把PC端算法照搬而是要在精度和速度之间做取舍——球体近似法就是这个取舍的答案精度够用计算够快落地够稳。
