一种小型水下机械手位置角度测量方法
【技术领域】
[0001] 本发明涉及的是一种水下机械手位置角度测量方法，具体地说是一种为水下机械 手末端位置精度以及关节角度精度测量方法。
【背景技术】
[0002] 随着陆地资源的日渐减少，人类开发海洋的步伐越来越快。自主式水下机器人及 其作业系统无人情况下在深海进行探测、开发的载体；随着其工作范围，作业要求的不断提 高，对其作业工具也提出了更高的要求。基于水下机械手作业任务高精度的要求，需要对水 下机械手运动控制精度进行验证测量。
[0003] 目前直接以测量水下机械手的末端位置角度精度为目的的测量方法相对较少，但 相关论文上描述的方法可以通过测量结果处理间接的得到机械手末端精度。如中文期刊海 洋工程（THE OCEAN ENGINEERING)刊登的"水下机械手接近觉系统研究"文章中，根据水下 机械手的工作需要，设计了一套高精度的水下超声波定位系统。采用超声波检测距离、三 探头阵列检测方位的方案，使用超低功耗单片机MSP430精确计算超声波往返时间和控制 三探头轮流工作，通过对三个探头的测量数据进行处理，实现目标测距及定位。如中文期 刊北京航空航天大学学报刊登的"基于CCD和超声的物体位姿检测方法及精度分析"文章 中，介绍了用单个CCD摄像机和超声传感器相结合检测物体位姿的方法。首先由CCD摄像 机采集一幅图像，获取物体上目标点在图像坐标系中的理想坐标，并由此确定该点投影矢 量（连接物体点和对应图像点的直线）的方向；然后控制机器人运动，使超声传感器的z 轴与求得的投影矢量重合；最后用超声传感器测出投影矢量的长度，确定目标点在摄像机 坐标系中的坐标。上述文章可通过各自方法得到末端运行坐标用以测量机械手末端精度， 上述方法所用的高精度超声探头以及CCD摄像机成本较高。
【发明内容】
[0004] 本发明的目的是提供一种测量过程方便简单、计算处理过程精简且测量成本低， 能有效解决水下机械手末端位置角度精度测量的小型水下机械手位置角度测量方法。
[0005] 本发明的目的是这样实现的：
[0006] (1)首先建立该方法测量时所用的基坐标系以及测量坐标系；以机械手安装固定 点为原点建立机械手基坐标系（O-XYZ)，机械手完全展开时末端点与安装固定点连线方向 为X轴，纵向垂直于X轴方向为Y轴，水平面垂直于X轴方向为Z轴，坐标系X、Y、Z轴的建 立遵循右手法则；将三个拉绳位移传感器安装组成三维位置测量装置，传感器之间的相对 位置在测量过程中不变并且安装位置在空间中呈"〈"型固定，基于该装置可建立一测量坐 标系O-XcYcZc,坐标系X、Y、Z轴与基坐标系相同。
[0007] (2)组成三维位置测量装置的各拉绳位移传感器两两之间相对位置为a、b、c (a、 b、c长度可由拉绳位移传感器测量），可知①、②、③拉绳位移传感器在坐标系O-XcYcZ c中的 位置分别为：①：[0, 0, -acos ( Θ )];②：[0, asin( Θ )，0];③：[0, 0, -acos ( Θ )-b]，其中： (3〇3(0) = (&2+132-(^/ 2&13，&、13、(;为各传感器之间安装的相对位置。由于三维位置测量装 置的三根测量线末端与机械手末端连接，机械手末端运动到测量坐标系下任意点，传感器 可以得到相应的拉伸长度。
[0008] (3)给定空间基坐标系中A[px，py，pz](理论值），针对机械手的自由度为 i(i多2)，通过传统的逆运动学方法求解得出机械手各关节的关节角0。到Θ i，即各关节角 转动角度的期望值，并将机械手各关节转动到期望值角度，使机械手末端运动到期望位置， 此时三维位置测量装置中各传感器拉伸值为Q、L2、L3;由于测量坐标系O-XJcZp Q、L2、L3均以确定，因此可建立如下所示方程组：
[0009] pcx2+pcy2+ (pcz+acos ( Θ ))2= L !2
[0010] Pcx2+ (pcy-asin ( Θ )) 2+pcz2= L 22
[0011] Pcx2+Pcy2+ (pcz+acos ( θ ) +b)2= L 32
[0012] 通过对上述方程组进行求解，可计算得到点A在测量坐标系0-&\4中的位置 [Pcx，Pcy，Pcz](头际值）。
[0013] (4)测量坐标系0-&\4通过坐标系旋转变换以及平移转换可得到基坐标系 0-ΧΥΖ，其旋转变换矩阵和平移转换矩阵分别为 在测量坐标系O-XeYeZe中的位置[p ^ Pq，PtJ可通过下式转换到基坐标系O-XYZ下表示：
[0015] 式中：AC为点A在测量坐标系O-X JcZc下矩阵A c= [p pcy p Jτ，A。为点A在基 坐标系O-XYZ下矩阵A。= [pM Pciy ρJt，并通过此变换矩阵式即可得出实际值在基坐标系 O-XYZ 下坐标[Pcix，Pciy, PtJ。
[0016] (5)在基坐标系下，将解算出来的点A坐标实际值[pM，Pciy，ρ J与理论值[px，py， Pz]比较，即可得出末端位置定位误差，同时关节角度误差P可通过下式计算得出：
%两点之间的误差距离，即为末 端位置定位误差，理论值点A[px，py，pz]到原点0距离
[0019] 三维位置测量装置（测量坐标系）可以整体移动以适应作业空间较大的机械手末 端精度测量。
[0020] 本发明适用于水下机械手关节为转动副，求解过程中当存在多重解情况，应采用 "最短行程"准则择优选取，即保证每个关节的转动量变化最小。
[0021] 需进行多次测量，得到多组机械手末端定位误差，并求取平均定位误差，再求取关 节角度定位误差⑦.α
[0022] 水下机械手位置角度的测量方法同样适用于陆地机械手使用。
[0023] 本发明提供了 一种有效解决水下机械手末端位置角度精度测量的方法，该方法测 量过程方便简单、计算处理过程精简且测量成本低；测量原理为通过本专利的三维测量装 置得到机械手末端在测量坐标系下的实际值坐标，但可将测量的实际值坐标通过坐标转换 到理论值基坐标系中，比较得到机械手末端运动的位置误差。
[0024] 本发明的有益效果为：
[0025] 1、提供一种机械手末端位置角度精度测量方法，由于测量装置拉绳位移传感器能 适用于水下测量，所以该方法适用于水下作业机械手。
[0026] 2、该方法测量过程简单、计算处理过程精简、测量所使用拉绳位移传感器成本低； 而且能快速的进行多次测量，求取平均误差；测量坐标系可以整体移动以适应作业空间较 大的机械手末端精度测量。
[0027] 3、通过该测量方法的平均误差结果，可通过后续的计算处理得到机械手各关节角 度定位精度。
【附图说明】
[0028] 图1本发明测量方法流程图；
[0029] 图2三维测量装置安装测量原理图；
[0030] 图3坐标系转换原理图；
[0031] 图4机械手关节角度误差计算原理图；
[0032] 图5三维测量装置移动测量原理图。
【具体实施方式】
[0033] 下面结合附图对本发明专利做进一步详述。
[0034] 如图1所示，本发明测量方法的测量流程和实现步骤为：
[0035] 1)首先基于本发明的测量原理，建立该方法测量时所用的基坐标系以及测量坐标 系；以机械手安装固定点为原点建立机械手基坐标系（O-XYZ)，将三个拉绳位移传感器安 装组成三维位置测量装置，并基于该装置可建立一测量坐标系0-WZ。。基坐标系（O-XYZ) 以机械手完全展开时末端点与安装固定点连线方向为X轴，纵向垂直于X轴方向为Y轴，水 平面垂直于X轴方向为Z轴，坐标系X、Y、Z轴的建立遵循右手法则；测量坐标系O-XeYcZ c, 坐标系X、Y、Z轴与基坐标系相同。三维位置测量装置的三根测量线末端与机械手末端连 接，机械手末端运动到测量坐标系下任意点，传感器可以得到相应的拉伸长度。
[0036]