CN119407791A 基于改进人工势场法的双臂碰撞检测及避障路径规划方法 （南京天创电子技术有限公司）

门大街23号金地威新雨花创新中心B基于改进人工势场法的双臂碰撞检测及避基于改进人工势场法的双臂碰撞检测及避障路22.根据权利要求1所述的基于改进人工势场法的双臂碰撞检测及避障路径规划方法，3.根据权利要求1所述的基于改进人工势场法的双臂碰撞检测及避障路径规划方法，将环境障碍物以及另一机械臂的关节球体和连杆圆柱体作为当前机械对当前机械臂的每个关节球体，计算其到环境障碍物、另一机械臂的针对关节球体的斥力距离计算包括：采用点到长方体的最短物长方体之间的斥力距离，采用点到点的最短距离计算关节球体与障碍物球体的斥力距到圆柱体之间的最短距离计算关节球体与另一机械臂针对连杆圆柱体的斥力距离计算包括：采用圆柱体到长方体的最4.根据权利要求3所述的基于改进人工势场法的双臂碰撞检测及避障路径规划方法，3向上的距离分量获得；所述点到点的最短距离采用两个球体之间法线方向体到圆柱体之间的距离通过计算各圆柱体中心点之间的直所述点到长方体之间的距离计算方法为：以长方体中心作为斥该点在长方体坐标系下的表示为pc=T⃞ep=(xc,y,zc)，点到长方体中心点的长度为;圆柱体斥力距离，假设圆柱体中心点在世界坐标系下的位姿矩阵为Tcylinder，半径为该点在圆柱体坐标系下的表示点到圆柱体中心点的长度为4。5.根据权利要求4所述的基于改进人工势场法的双臂碰撞检测及避障路径规划方法，。6.根据权利要求5所述的基于改进人工势场法的双臂碰撞检测及避障路径规划方法，;;7.根据权利要求6所述的基于改进人工势场法的双臂碰撞检测及避障路径规划方法，"Fsum="Fan+z""Frep(i)(k=l,r)，其中，"为主臂关节数量。8.根据权利要求7所述的基于改进人工势场法的双臂碰撞检测及避障路径规划方法，5;9.根据权利要求8所述的基于改进人工势场法的双臂碰撞检测及避障路径规划方法，6前的AABB包围盒和OBB包围盒不能直接计算出物体到包围盒的最短距离，仅用于判断物体[0006]技术方案：本发明所述基于改进人工势场法的双臂碰撞检测及避障路径规划方7[0020]将环境障碍物以及另一机械臂的关节球体和连杆圆柱体作为当前机械臂的障碍点到圆柱体之间的最短距离计算关节球体与另一机械臂述圆柱体到圆柱体之间的距离通过计算各圆柱体中心点之间的直方体中心点在世界坐标系下的位姿矩阵为Tube，长宽高分别为L,W,H，空间内一点为[0026]该点在长方体坐标系下的表示为pc=T⃞ep=(xc,y,zc)，点到长方体中心点的长8;[0029]该点在圆柱体坐标系下的表示。9。[0057]图4是本发明实施例2中障碍物和双机械臂关节连杆包络示意图，5为左臂碰撞几[0063]图10是本发明实施例2中双机械臂末端与目标点距离对比分析，左子图为传统人[0065]实施例1：如图1所示基于改进人工势场法的双臂碰撞检系，通过双臂的位姿变换矩阵可得到世界坐标系下的物体在右臂的基座坐标系下的表示。左臂第i(i=2,3,…n)个关节的位置pu(i)，左臂第i(i=2,3,…n-。向斥力场中心方向与障碍物的距离作为斥力距离，圆柱体的斥力距离计算方法同样如此。宽高分别为L,w,H，空间内一点为p(x,y,z)，计算该点在长方体坐标系下的表示：pc=T⃞ep=(xc,y,zc)，点到长方体中心点的长度为，则点到长方体的斥[0082]点到圆柱体的斥力距离，假设圆柱体中心点在世界坐标系下的位姿矩阵为。斥力势能受机械臂末端与目标点距离影响，可有效解决目标点在障碍物附近不可达的问[0096]S54、计算机械臂末端作用力"Fsum="Fan+z""Frep(i)(k=l,r)，其中，"为机械臂