[发明专利]一种小型水下机械手位置角度测量方法

摘要

本发明提供的是一种小型水下机械手位置角度测量方法。基于三个拉绳位移传感器安装组成三维位置测量装置，建立其测量坐标系(O‑XCYCZC)，并在机械手安装固定点建立基坐标系(O‑XYZ)。本方法原理为通过三维测量装置得到机械手末端在测量坐标系下的实际值坐标，将实际值坐标通过坐标转换到基坐标系中，比较得到机械手末端运动位置误差。本发明提供了一种测量过程方便简单、计算处理过程精简且成本低的水下机械手位置角度精度测量方法，适合于水下机械手在水池环境下进行三维位置测量以及末端角度精度测量。

主权项

一种小型水下机械手位置角度测量方法，其特征是：(1)建立坐标系：(1.1)基坐标系O‑XYZ：以机械手安装固定点为原点建立机械手基坐标系O‑XYZ，机械手完全展开时末端点与安装固定点连线方向为X轴、纵向垂直于X轴方向为Y轴、水平面垂直于X轴方向为Z轴；(1.2)测量坐标系O‑XCYCZC：将三个拉绳位移传感器安装组成三维位置测量装置，传感器之间的相对位置在测量过程中不变并且安装位置在空间中呈“<”型固定，基于所述三维位置测量装置建立一测量坐标系O‑XCYCZC，测量坐标系X、Y、Z轴与基坐标系相同，各拉绳位移传感器两两之间相对位置为a、b、c，第一至第三拉绳位移传感器在坐标系O‑XCYCZC中的位置分别为：第一绳位移传感器：[0，0，‑acos(θ)]；第二拉绳位移传感器：[0，asin(θ)，0]；第三拉绳位移传感器：[0，0，‑acos(θ)‑b]，其中：cos(θ)＝(a2+b2‑c2)/2ab；(2)拉绳位移传感器组成的三维位置测量装置的三根测量线末端与机械手末端连接，机械手末端运动到测量坐标系下任意点，拉绳位移传感器得到相应的拉伸长度；(3)基坐标系下，给点空间中任意点A，机械手末端运动到A点时，末端在测量坐标系中坐标求解过程如下：(3.1)机械手逆运动学反解：基坐标系下，空间中任意点A[px，py，pz]，针对机械手的自由度为i，i≥2，通过逆运动学方法求解得出机械手各关节的关节角θ0到θi，即各关节角转动角度的期望值，并将机械手各关节转动到期望值角度，使机械手末端运动到期望位置；(3.2)测量坐标系坐标求解：机械手末端运动到期望位置时，三维位置测量装置中各拉绳位移传感器拉伸值为L1、L2、L3；测量坐标系O‑XCYCZC、L1、L2、L3均以确定，pcx2+pcy2+(pcz+acos(θ))2＝L12pcx2+(pcy‑asin(θ))2+pcz2＝L22pcx2+pcy2+(pcz+acos(θ)+b)2＝L32式中：pcx、pcy、pcz为测量坐标值，L1、L2、L3为传感器拉伸长度，θ为步骤(1.2)中所述角度值，求解上述方程组，得到点A在测量坐标系O‑XCYCZC中的位置[pcx，pcy，pcz]；(4)测量坐标系O‑XCYCZC与基坐标系O‑XYZ存在坐标变换，通过坐标变换矩阵，求出点A从测量坐标系变换到基坐标系下的坐标；(4.1)坐标变换过程：测量坐标系O‑XCYCZC与基坐标系O‑XYZ变换过程为测量坐标系先绕Z轴旋转180°，再通过平移变换得到基坐标系，X、Y、Z轴平移量分别为d、e、f；测量坐标系O‑XCYCZC到基坐标系O‑XYZ的旋转变换矩阵为测量坐标系O‑XCYCZC相对于基坐标系O‑XYZ的平移矩阵为(4.2)坐标计算过程：点A在测量坐标系O‑XCYCZC的坐标通过下式转换到基坐标系O‑XYZ下表示，坐标转换式为：式中：AC为点A在测量坐标系O‑XCYCZC下矩阵AC＝[pcx pcy pcz]T，AO为点A在基坐标系O‑XYZ下矩阵AO＝[pox poy poz]T，通过上式得出实际值在基坐标系O‑XYZ下坐标[pox，poy，poz]；(5)在基坐标系下，将解算出来的点A坐标实际值[pox，poy，poz]与理论值A[px，py，pz]比较，得出末端位置定位误差，同时关节角度误差通过下式计算得出：式中：为关节角度定位误差，为两点之间的误差距离，即为末端位置定位误差，理论值点A[px，py，pz]到原点O距离