[发明专利]一种用于测量工程机械臂的位姿的方法和系统

说明书

本发明公开了一种用于测量工程机械臂的位姿的方法及系统，该方法包括：标靶安装步骤，将至少三个标靶安装在所述工程机械臂的末端，使这至少三个标靶与该机械臂臂架的中轴线的设定点构成预定位置关系；参数获取步骤，获取所述至少三个标靶在机械臂载体坐标系下的位置信息以及所述标靶的结构尺寸参数和安装尺寸参数；机械臂末端位姿计算步骤，利用所述预定位置关系和获取的位置信息及结构尺寸参数和安装尺寸参数，计算得到该机械臂末端当前在机械臂载体坐标系下的包括位置坐标和姿态的位姿信息。通过本发明的方法能够实时准确测量机械臂的位置和姿态，辅助作业人员用来控制该机械臂的运动，更好地完成作业任务。

技术领域

本发明涉及工程机械领域，尤其涉及一种用于测量工程机械臂的位姿的方法和系统。

背景技术

在隧道工程施工过程中，为了防止围岩变形和破碎，通常需要采用架设拱架、安装锚杆、挂钢筋网、喷射混凝土等方法对围岩进行加固。以隧道拱架多功能作业台车为例，该台车是集拱架定位、安装、焊接于一体的自动化隧道施工装备，作为其核心的作业臂架是一个10自由度的串联机构，通过调整关节位姿使末端作业装置实现预定的位置和姿态是多功能作业台车的基本要求，同时也是其研制中的关键技术。

多功能作业台车臂架长、关节多、自身重量大，易发生柔性变形，且加工与装配后的各个部件存在尺寸误差，实现末端作业装置的精确定位具有很大的技术难度，因此需要一种能够快速、较为精确地获取机械臂末端位姿的方法。

发明内容

本发明所要解决的技术问题之一是需要提供一种用于测量工程机械臂的位姿的方法和系统。

为了解决上述技术问题，本申请的实施例首先提供了一种用于测量工程机械臂的位姿的方法，该方法包括：标靶安装步骤，将至少三个标靶安装在所述工程机械臂的末端，使这至少三个标靶与该机械臂臂架的中轴线的设定点构成预定位置关系；参数获取步骤，获取所述至少三个标靶在机械臂载体坐标系下的位置信息以及所述标靶的结构尺寸参数和安装尺寸参数；机械臂末端位姿计算步骤，利用所述预定位置关系和获取的位置信息及结构尺寸参数和安装尺寸参数，计算得到该机械臂末端当前在机械臂载体坐标系下的包括位置坐标和姿态的位姿信息。

根据本发明的一个实施例，在所述机械臂末端位姿计算步骤中，包括如下步骤：姿态计算步骤，确定标靶坐标系相对于末端臂架关节坐标系的第一姿态矩阵，以及所述标靶坐标系相对于机械臂载体坐标系的第二姿态矩阵，根据第一姿态矩阵和第二姿态矩阵确定末端臂架关节坐标系相对于机械臂载体坐标系的姿态矩阵，从而得到该机械臂末端的姿态信息。

根据本发明的一个实施例，在所述机械臂末端位姿计算步骤中，还包括如下步骤：位置计算步骤，根据标靶的结构尺寸参数和安装尺寸参数，分别获取机械臂中轴线上的设定点和末端臂架关节中心点的距离以及该设定点在机械臂载体坐标系下的坐标，进而获取末端臂架关节中心点在机械臂载体坐标系下的坐标。

根据本发明的一个实施例，在姿态计算步骤中，根据三个标靶在机械臂载体坐标系下的坐标以及标靶坐标系与三个标靶之间的位置关系可求得所述第一姿态矩阵；根据获取的所述至少三个标靶的位置信息以及这至少三个标靶与该机械臂臂架的中轴线的设定点构成预定位置关系确定所述第二姿态矩阵。

根据本发明的一个实施例，在安装三个标靶时，所述预定位置关系为三个标靶以机械臂臂架的中轴线为中心，相邻两标靶与中轴线上的设定点之间连线彼此相差设定角度。

根据本发明的另一方面，还提供了一种用于测量工程机械臂的位姿的系统，该系统包括：至少三个标靶，其被安装在所述工程机械臂的末端，使这至少三个标靶与该机械臂臂架的中轴线的设定点构成预定位置关系；参数获取装置，其获取所述至少三个标靶在机械臂载体坐标系下的位置信息以及所述标靶的结构尺寸参数和安装尺寸参数；机械臂末端位姿计算装置，其利用所述预定位置关系和获取的位置信息及结构尺寸参数和安装尺寸参数，计算得到该机械臂末端当前在机械臂载体坐标系下的包括位置坐标和姿态的位姿信息。