【技术实现步骤摘要】
一种双臂自碰撞检测方法和计算机可读存储介质
本专利技术涉及机器人领域，具体涉及一种双臂自碰撞检测方法和计算机可读存储介质。
技术介绍
当前对于自碰撞检测的方式主要有两种：一是基于力矩传感器检测。二是基于几何模型的碰撞检测，用基本几何体包围实际模型的碰撞检测。然而，现有的自碰撞检测的方式存在以下不足：1)力矩传感器的检测方法，成本过高，且在机器人设计阶段就需要考虑力矩传感器的安装问题，也就是说，该方法并不一定适用于所有现有的机器人；再者，单纯的力矩检测在机器人调试过程中，会出现很多误判的情况；其三，基于力矩传感器的检测，检出时碰撞已经发生，无法起到预防作用。2)几何模型碰撞检测方法通过基本几何体包络实际模型，不足之处在于其碰撞精度取决于包络后的模型与实际模型的紧密程度。本方法是几何模型碰撞检测方法的一种扩展，更通用的方式。在机器人领域，特别是双臂协作机器人，需要保证双臂在给定的任务规划过程中左臂与左臂、右臂、本体或者右臂与右臂、本体之间不发生碰撞的约束情况，此时就需要一种可以在线且实时的碰撞检测算法来确保机器人运动过程中的安全...
【技术保护点】
1.一种双臂自碰撞检测方法，其特征在于，包括：/n构建碰撞模型；/n更新空间位置点信息；/n执行碰撞检测；/n其中，所述碰撞模型包括：外层包围盒和内层实际模型；所述碰撞检测包括外层包围盒碰撞检测和内层实际模型碰撞检测。/n
【技术特征摘要】
1.一种双臂自碰撞检测方法，其特征在于，包括：
构建碰撞模型；
更新空间位置点信息；
执行碰撞检测；
其中，所述碰撞模型包括：外层包围盒和内层实际模型；所述碰撞检测包括外层包围盒碰撞检测和内层实际模型碰撞检测。
2.根据权利要求1所述的双臂自碰撞检测方法，其特征在于，外层包围盒碰撞检测优先于内层实际模型碰撞检测，并且仅在外层包围盒碰撞时才执行内层实际模型碰撞检测。
3.根据权利要求2所述的双臂自碰撞检测方法，其特征在于，所述构建外层包围盒包括：
设左臂的关节数目记为m，右臂的关节数目记为n，上标表示左臂，上标r表示右臂，表示左臂第j关节中心点；
设左臂、右臂的基坐标系分别为：
设左臂、右臂各关节初始关节角分别为：
已知双臂模型尺寸数据：左臂关节连杆三维尺寸半径三角面网格数据右臂关节连杆三维尺寸半径三角面网格数据本体：三维尺寸So、半径Ro、三角面网格数据Do；
设包围盒八个顶点的坐标分别为：(x1，y1，z1)，(x2，y2，z2)，…，(x8，y8，z8)，则根据空间向量法获得膨胀后的包围盒；其中，根据空间向量法获得膨胀后的包围盒包括：设V0为膨胀前的顶点，V0沿x方向单位向量nx为：
其中，Kx＝[x1y1z1]-[x2y2z2]；
V0沿y方向单位向量ny为：
Ky＝[x1y1z1]-[x5y5z5]；
V0顶点沿z方向单位向量nz为：
Kz＝[x1y1z1]-[x4y4z4]；
膨胀后的顶点：
4.根据权利要求3所述的双臂自碰撞检测方法，其特征在于，更新模型空间位置点信息包括：
采用下式计算臂本体坐标系与双臂基坐标系位置关系的齐次矩阵：
其中，Pos2Mat()为将笛卡尔空间位姿转化为对应齐次矩阵的函数；为左臂本体坐标系到双臂基坐标系的位姿，为经过Pos2Mat()函数计算后得到的左臂本体坐标系与双臂基坐标系的齐次变换矩阵；为右臂本体坐标系到双臂基坐标系的位姿，为经过Pos2Mat()函数计算后得到的右臂本体坐标系与双臂基坐标系的齐次变换矩阵；
采用下式计算各关节在臂坐标系下位置关系的齐次矩阵：
其中，fk()为由初始关节角求得各关节处的齐次坐标变换矩阵的函数，k为关节号，k∈[1，m+1]，k’∈[1，n+1]；Ql、Qr分别为左臂、右臂各关节初始关节角；分别为臂、右臂各关节中心点相对于臂本体的位姿关系；
采用下式，利用已求得的臂与双臂基坐标系齐次矩阵、关节与臂坐标系齐次矩阵，计算关节与双臂基坐标系齐次矩阵：
其中，分别为左臂、右臂各关节中心点的相对于双臂基坐标系的齐次坐标变换矩阵；
采用下式实时更新左臂、右臂包围盒空间位置；
<...
【专利技术属性】
技术研发人员：杨跞，宗成星，孙超，许楠，李法设，
申请(专利权)人：中科新松有限公司，
类型：发明
国别省市：上海;31
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。