[发明专利]自动换刀机械手臂运动轨迹的测量方法和装置

摘要

本发明一种自动换刀机械手臂运动轨迹的测量方法和装置属于加工中心自动换刀系统机械手臂的空间运动轨迹跟踪测量领域。该方法是一种基于二维PSD的非接触式三维在线测量方法，作为被测目标的光源安装在被测量ATC机械手臂上，光源通过透镜投射在由两个位置敏感探测器组成的PSD相机的二维感应屏上形成光斑，两个PSD相机在光斑的激发下各自输出二维坐标值，经过信号处理器后输出与坐标位置对应的电压值，计算机通过电压采集卡将相应信号采集保存。该方法响应速度快，满足实时在线测量要求。通过机械手臂的运动轨迹可以判断换刀过程中的定位精度、运动平稳性等运动状态，为刀库可靠性检测提供量化方法。

主权项

一种自动换刀机械手臂运动轨迹的测量方法，其特征在于：该方法是一种基于二维PSD的非接触式三维在线测量方法，作为被测目标的光源安装在被测量ATC机械手臂上，光源通过透镜投射在由两个位置敏感探测器组成的PSD相机的二维感应屏上形成光斑，两个PSD相机在光斑的激发下各自输出二维坐标值，经过信号处理器后输出与坐标位置对应的电压值，计算机通过电压采集卡将相应信号采集保存，由两组采集到的二维坐标值反算出被测光源的三维坐标值，最后，依据高频采集的数据计算出一系列离散点构成机械手臂的运动轨迹；具体步骤如下：1）PSD相机内外参数的测量设被测目标光源所在的坐标系为世界坐标系W，投射光斑所在坐标系为PSD坐标系S，设世界坐标系W下某点P (XW，YW，ZW)，其对应的投射点P’在PSD坐标系S下的坐标为P’(u,v)，二者之间的转换关系为： Z L μ ν 1 = f x 0 μ 0 0 0 f y ν 0 0 0 0 1 0 · R T 0 1 · X W Y W Z W 1 = A 0 · R T 0 1 · X W Y W Z W 1 - - - ( 1 ) 其中，A为内参数矩阵，其包含了焦距f和偏移量(u0,v0)，R、T分别为世界坐标系到镜头坐标系的旋转矩阵和平移矩阵，[R T]为PSD相机的外参数矩阵；使用平面模板37来测量PSD相机的内外参数，设Zw=0； Z L μ ν 1 = f x 0 μ 0 0 0 f y ν 0 0 0 0 1 0 · R 1 R 2 T 0 0 1 · X W Y W 1 = H · X W Y W 1 - - - ( 2 ) 根据张氏标定法求解内外参数，固定好PSD相机和模板后启动控制电路（38），从光源阵列（36）左上角起扫描式依次点亮光源，同时采集相应的PSD的输出坐标值，直到最后一个光源测量完毕，将采集到的数据和相应的光源的 在世界坐标系下的坐标值代入下述方程： X W Y W 1 0 0 0 - μ · X W - μ · Y W - μ 0 0 0 X W Y W 1 - ν · X W - ν · Y W - ν · h 11 h 12 h 13 h 21 h 22 h 21 h 31 h 32 h 33 = 0 0 - - - ( 3 ) 平面模板（37）中的光源数量足够多时，通过最小二乘法求解出单应性矩阵H，改变相机或者模板的方位，就能求出不同单应性矩阵，将多组H代入下述方程： V 12 T ( V 11 - V 22 ) T · b = 0 - - - ( 4 ) 其中 V ij = h i 1 · h j 1 h i 1 · h j 2 + h i 2 · h j 1 h i 1 · h j 3 + h i 3 · h j 1 h i 2 · h j 2 h i 2 · h j 3 + h i 3 · h j 2 h i 3 · h j 3 T ，b是对称矩阵B的六个独立元素构成的向量； B = ( A - 1 ) T A - 1 = 1 f x 2 0 - μ 0 f x 2 0 1 f y 2 - ν 0 f y 2 - μ 0 f x 2 - ν 0 f y 2 μ 0 2 f x 2 + ν 0 2 f y 2 + 1 当改变的方位次数足够多时，通过最小二乘法求出b，并反算出相机内参数矩阵A，将A代入下式：R1=A‑1H1，R2=A‑1H2，T=A‑1H3                       (5)然后通过将R1和R2通过正交扩展出R3 ，求出相机外参数矩阵[R T]；2）目标光点三维坐标的计算散射目标光源发出的光束经过镜头汇聚后投影在PSD相机感应屏上，世界 坐标系下目标光源中心坐标（XW，YW，ZW）与PSD坐标系下投影光斑中心坐标（μ，ν）的关系如下式： m 1 - m 9 μ m 2 - m 10 μ m 3 - m 11 μ m 5 - m 9 ν m 6 - m 10 ν m 7 - m 11 ν · X W Y W Z W = m 12 μ - m 4 m 12 ν - m 8 - - - ( 6 ) 其中， M = m 1 m 2 m 3 m 4 m 5 m 6 m 7 m 8 m 9 m 10 m 11 m 12 = A 0 · R T 0 1 利用单个PSD相机输出的二维坐标可以列出如上式的两个方程，两个PSD相机同时感应同个目标光源，可以列出四个方程并通过最小二乘法求解出目标光源中心在世界坐标系下的坐标值；3）机械手臂运动轨迹的测量将目标光源安装在加工中心ATC的机械手臂上，其随换刀动作在空间形成机械手臂的运动轨迹，运动轨迹测量采用目标光源的三维坐标值计算和离散点曲线拟合方法；PSD相机的感应面是一个二维平面，投影光斑的二维坐标值与目标光点的三维坐标值通过PSD相机的内外参数矩阵来表示，两个二维PSD相机输出的坐标值可以反解出目标点的三维坐标，一系列离散目标点拟合出加工过程中机械手臂的运动轨迹。