一种小型水下机械手位置角度测量方法技术

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

【技术实现步骤摘要】

本专利技术涉及的是一种水下机械手位置角度测量方法，具体地说是一种为水下机械手末端位置精度以及关节角度精度测量方法。
技术介绍
随着陆地资源的日渐减少，人类开发海洋的步伐越来越快。自主式水下机器人及其作业系统无人情况下在深海进行探测、开发的载体；随着其工作范围，作业要求的不断提高，对其作业工具也提出了更高的要求。基于水下机械手作业任务高精度的要求，需要对水下机械手运动控制精度进行验证测量。目前直接以测量水下机械手的末端位置角度精度为目的的测量方法相对较少，但相关论文上描述的方法可以通过测量结果处理间接的得到机械手末端精度。如中文期刊海洋工程(THEOCEANENGINEERING)刊登的“水下机械手接近觉系统研究”文章中，根据水下机械手的工作需要,设计了一套高精度的水下超声波定位系统。采用超声波检测距离、三探头阵列检测方位的方案,使用超低功耗单片机MSP430精确计算超声波往返时间和控制三探头轮流工作,通过对三个探头的测量数据进行处理,实现目标测距及定位。如中文期刊北京航空航天大学学报刊登的“基于CCD和超声的物体位姿检测方法及精度分析”文章中，介绍了用单个CCD摄像机和超声传感器相结合检测物体位姿的方法。首先由CCD摄像机采集一幅图像，获取物体上目标点在图像坐标系中的理想坐标，并由此确定该点投影矢量(连接物体点和对应图像点的直线)的方向；然后控制机器人运动,使超声传感器的z轴与求得的投影矢量重合；最后用超声传感器测出投影矢量的长度,确定目标点在摄像机坐标系中的坐标。上述文章可通过各自方法得到末端运行坐标用以测量机械手末端精度，上述方法所用的高精度超声探头以及CCD摄像机成本较高。
技术实现思路
本专利技术的目的是提供一种测量过程方便简单、计算处理过程精简且测量成本低，能有效解决水下机械手末端位置角度精度测量的小型水下机械手位置角度测量方法。本专利技术的目的是这样实现的：(1)首先建立该方法测量时所用的基坐标系以及测量坐标系；以机械手安装固定点为原点建立机械手基坐标系(O-XYZ)，机械手完全展开时末端点与安装固定点连线方向为X轴，纵向垂直于X轴方向为Y轴，水平面垂直于X轴方向为Z轴，坐标系X、Y、Z轴的建立遵循右手法则；将三个拉绳位移传感器安装组成三维位置测量装置，传感器之间的相对位置在测量过程中不变并且安装位置在空间中呈“<”型固定，基于该装置可建立一测量坐标系O-XCYCZC，坐标系X、Y、Z轴与基坐标系相同。(2)组成三维位置测量装置的各拉绳位移传感器两两之间相对位置为a、b、c(a、b、c长度可由拉绳位移传感器测量)，可知①、②、③拉绳位移传感器在坐标系O-XCYCZC中的位置分别为：①：[0,0,-acos(θ)]；②：[0,asin(θ),0]；③：[0,0,-acos(θ)-b]，其中：cos(θ)＝(a2+b2-c2)/2ab，a、b、c为各传感器之间安装的相对位置。由于三维位置测量装置的三根测量线末端与机械手末端连接，机械手末端运动到测量坐标系下任意点，传感器可以得到相应的拉伸长度。(3)给定空间基坐标系中A[px，py，pz](理论值)，针对机械手的自由度为i(i≥2)，通过传统的逆运动学方法求解得出机械手各关节的关节角θ0到θi，即各关节角转动角度的期望值，并将机械手各关节转动到期望值角度，使机械手末端运动到期望位置，此时三维位置测量装置中各传感器拉伸值为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通过对上述方程组进行求解，可计算得到点A在测量坐标系O-XCYCZC中的位置[pcx，pcy，pcz](实际值)。(4)测量坐标系O-XCYCZC通过坐标系旋转变换以及平移转换可得到基坐标系O-XYZ，其旋转变换矩阵和平移转换矩阵分别为TCO=-1000-10001,PCA=defT;]]>点A在测量坐标系O-XCYCZC中的位置[pcx，pcy，pcz]可通过下式转换到基坐标系O-XYZ下表示：AO=TCO·AC+PCA]]>式中：AC为点A在测量坐标系O-XCYCZC下矩阵AC＝[pcxpcypcz]T，AO为点A在基坐标系O-XYZ下矩阵AO＝[poxpoypoz]T，并通过此变换矩阵式即可得出实际值在基坐标系O-XYZ下坐标[pox，poy，poz]。(5)在基坐标系下，将解算出来的点A坐标实际值[pox，poy，poz]与理论值[px，py，pz]比较，即可得出末端位置定位误差，同时关节角度误差可通过下式计算得出：式中：为两点之间的误差距离，即为末端位置定位误差，理论值点A[px，py，pz]到原点O距离三维位置测量装置(测量坐标系)可以整体移动以适应作业空间较大的机械手末端精度测量。本专利技术适用于水下机械手关节为转动副，求解过程中当存在多重解情况，应采用“最短行程”准则择优选取，即保证每个关节的转动量变化最小。需进行多次测量，得到多组机械手末端定位误差，并求取平均定位误差，再求取关节角度定位误差水下机械手位置角度的测量方法同样适用于陆地机械手使用。本专利技术提供了一种有效解决水下机械手末端位置角度精度测量的方法，该方法测量过程方便简单、计算处理过程精简且测量成本低；测量原理为通过本专利的三维测量装置得到机械手末端在测量坐标系下的实际值坐标，但可将测量的实际值坐标通过坐标转换到理论值基坐标系中，比较得到机械手末端运动的位置误差。本专利技术的有益效果为：1、提供一种机械手末端位置角度精度测量方法，由于测量装置拉绳位移传感器能适用于水下测量，所以该方法适用于水下作业机械手。2、该方法测量过程简单、计算处理过程精简、测量所使用拉绳位移传感器成本低；而且能快速的进行多次测量，求取平均误差；测量坐标系可以整体移动以适应作业空间较大的机械手末端精度测量。3、通过该测量方法的平均误差结果，可通过后续的计算处理得到机械手各关节角度定位精度。附图说明图1本专利技术测量方法流程图；图2三维测量装置安装测量原理图；图3坐标系转换原理图；图4机械手关节角度误差计算原理图；图5三维测量装置移动测量原理图。具体实施方式下面结合附图对本专利技术专利做进一步详述。如图1所示，本专利技术测量方法的测量流程和实现步骤为：1)首先基于本专利技术的测量本文档来自技高网...

【技术保护点】
一种小型水下机械手位置角度测量方法，其特征是：(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从测量坐标系变换到基坐标系下的坐标；CO=-1000-10001,]]>测量坐标系O‑XCYCZC相对于基坐标系O‑XYZ的平移矩阵为PCA=defT;]]>(4.2)坐标计算过程：点A在测量坐标系O‑XCYCZC的坐标通过下式转换到基坐标系O‑XYZ下表示，坐标转换式为：CO·AC+PCA]]>式中：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]比较，得出末端位置定位误差，同时关节角度误差通过下式计算得出：式中：为关节角度定位误差，d=(px-pox)2+(py-poy)2+(pz-poz)2]]>为两点之间的误差距离，即为末端位置定位误差，理论值点A[px，py，pz]到原点O距离...

【技术特征摘要】
2015.03.13 CN 20151011232001.一种小型水下机械手位置角度测量方法，其特征是：
(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存在坐标变...

【专利技术属性】
技术研发人员：张铭钧，杨超，马小委，王玉甲，刘兵，姚峰，郭冠群，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙江;23