基于双PSD 的机械手运动轨迹跟踪测量装置*

卢晓红 王文韬 司立坤 韩鹏卓 任宗金

(大连理工大学精密与特种加工教育部重点实验室，辽宁 大连 116024)

未来复杂的服役环境要求自动化设备空间机械手必须具备精确的末端定位和空间姿态控制［1］，而机械手装配间隙、承载变形等原因导致其末端执行器的位置与理论计算存在偏差，良好的运动学设计是解决上述问题的有效途径，而对机械手的运动轨迹、空间位姿的测量是合理规划、精确位姿控制及运动分析的必要手段。

目前对空间机械手的运动轨迹和位姿测量一般采用接触式和非接触式两种方式。非接触方式响应速度快、测量过程简单，大大提高了测量效率，而且不增加被测量设备的承载负担，是目前较适合机械手位姿测量、复杂机构构件运动轨迹测量的方法。非接触方法主要有基于摄像技术的图像提取、双目立体视觉法、激光跟踪测量、激光三角测量、室内GPS 等方法。基于摄像技术的双目视觉测量方法用两台摄像机或照相机模拟人的双眼，获得有一定视差的两幅图像，计算机通过图像处理，运用双目视觉原理，就可以得到图像中空间目标的三维信息。如果将摄像机安装在机械手的末端，在运动过程中观测同一个固定目标，通过计算摄像机坐标系下固定目标的变化，反求出末端执行器的空间位置和姿态的变化［2-3］。Lau 等人提出的单站式5D 激光跟踪干涉测量系统［4］，该系统可以测量目标的X、Y、Z、俯仰角和偏摆角5 个自由度。奥地利维也纳工业大学的J.P.Prenninger 等人建立了一种单站式6 -D 激光跟踪测量系统［5］，用来实时动态地测量机器人关节末端运动的6 个自由度。清华大学的刘永东设计了三站激光跟踪坐标测量系统，实现了平面运动目标坐标的跟踪测量［6］。日本的Seiji AOYAGI 等人利用超声波测距的原理开发了一套超声波位姿测量系统［7］，该系统利用多边法测量空间三维坐标。室内GPS(indoor global positioning system，iGPS)的定位原理跟GPS 相同［8］，只是iGPS 的室内标定好的多个标定装置代替了GPS 的空间卫星［9］，iGPS 接收装置安装在工业机器人的末端执行器上，将末端的空间三维信息实时的传递给控制计算机。该测量装置适合大空间的跟踪测量，目前有应用在飞机机体焊接上的报道。位置敏感探测器(PSD)通过收集其电极输出的光电流就能连续地、实时地检测出入射光斑的重心位置。PSD 入射光斑位置的模拟信号是从位于光电二极管表面电阻层边缘的电极输出的，这种非寻址的探测器的优点是探测器和信号处理电路结构简单，且能实现连续的位置模拟信号输出，而不存在测量盲区［10］。与CCD 相比，PSD 具有响应速度快、感应面连续、成本低、传输速度快等优点，广泛应用在三维测量、角度姿态测量等领域。哈尔滨工业大学研制了用于空间飞行器对接的位置敏感器近程操作模拟器［11］，由PSD 为核心器件构成的光电传感系统在近程内对放置在空间飞行器上的合作目标的位置和姿态进行高精度测量。由激光器与位置敏感探测器PSD 组成的传感器被大量地用于机器人上，传感器可测量挠性机械臂的除杆长方向外的5 个自由度误差［12］。1993 年日本中京大学和松下共同研制了一种利用PSD 的三维视觉传感器，用于机器人中获取运动物体的三维形状［13］。

目前主流的运动轨迹和位姿测量手段是基于CCD 的多目视觉法和基于PSD 的光源阵列法。CCD摄像机传输的是图像信息，其测量精度很大程度上依赖高像素的照片，而高像素必定会影响相机的传输帧频，所以基于CCD 的测量在精度和响应速度上是矛盾的。与之相比，PSD 则不存在这样的问题，PSD 是依靠输出的电流或电压来计算目标光斑的位置，可以达到几十千赫的传输速度而不影响数据的精度。近年来，对PSD 器件的研究有很大进展，PSD 器件的线性度和稳定度也有很大改善［14］。PSD 器件自身的分辨率可达1 μm，陆军等用包括激光发射器、激光供电电源、PSD 器件等组成的PSD 位置测量系统进行二维测量，其平均控制误差为50 μm［15］。因此，在相同成本下，使用PSD 光源阵列法进行运动轨迹和位姿测量的应用具有较好的前景。

在实际应用中，很多的机械设备的移动速度都很快，加工中心的自动换刀机械手一次动作一般在2～5 s，而且是在三维空间内运动，运动空间受限，无法安装激光发射器。天津大学李兴达进行了基于PSD 的光点空间位置测量技术研究，距离12 m 处X 方向最大绝对误差为29 mm［16］。根据对PSD 相应光谱的研究和分析，发现近红外区是PSD 最敏感的范围，大约在900 nm，这就要求被测标靶光源的主要光谱分布也在这个区域。为了减少对被测量的自动化设备的负载及机械动作的影响，光源必须能够实现恒流驱动和无线控制。本文针对基于双PSD 实现空间点三维运动轨迹和位姿检测方面的关键技术展开研究，探索一种以大功率红外LED 为目标光源的非接触式三维位姿检测方法，解决有限空间内，复杂机构上点目标的运动轨迹实时在线测量、机器人末端执行器的空间位置及姿态的快速测量问题。

1 基于PSD 的运动轨迹和位姿测量原理

1.1 PSD 工作原理

二维枕型PSD 结构如图1 所示，二维枕型PSD 以感应面中心点为参考坐标系原点，入射光斑A 点的坐标值与4 个电极上的电流满足式(1)和式(2)。

式中:L 为光敏感面长度。从式(1)和式(2)可以看出，通过二维PSD 的4 个电极上的电流值就可以求解出入射光点在PSD 感光面上的平面位置信息。同理，利用2 个PSD 设备，就可以得到目标光源的三维位置信息。

1.2 基于PSD 的机械手运动轨迹测量装置的工作原理

使用单个PSD 的投影是基于针孔相机的线性映射关系。本文通过4 个坐标系统(如图2 所示)的定义及相互转换关系解释映射关系。

(1)世界坐标系:根据被测工件所在的自然环境选定的绝对坐标系;(2)镜头坐标系:以透镜中心OL为坐标原点，透镜光轴为ZL轴，XL、YL轴与PSD 坐标系的XS、YS轴平行;(3)图像坐标系:图像平面为PSD 理想的投影平面，原点O2为光轴与投影平面的交点，Xz、Yz轴与镜头坐标系的XL、YL轴平行，方向相反;(4)PSD 坐标系:其XSOSYS平面与图像的XZOIYZ平面共面，原点为PSD 传感器的物理中心，XS、YS轴与图像坐标系的XI、YZ轴方向相同，坐标原点相差一个偏移量。

镜头坐标系是世界坐标系经过旋转、平移的组合变换得到的，设旋转矩阵为R，平移矩阵为T。设空间P 点在世界坐标系下的坐标为(XW，YW，ZW)。P 点在世界坐标系中的坐标XwYwZw与PSD 坐标系中映射的二维坐标值(u，v)之间的对应关系如式(3)所示。

在计算机视觉中将相机的参数分为两大类，即内参数和外参数。内参数是相机与焦距及感应屏上像元分布有关。式(3)中A 称为相机的内参数矩阵。外参数是指相机在世界坐标系中的方位，可以通过R 和T获得，故称［R T］为外参数矩阵。

式(3)实质是空间一个光源点P 在世界坐标系中的三维坐标值(XWYWZW)与其透过工业镜头的光斑点在PSD 坐标系中的二维坐标值(u，v)之间的映射关系，可将此过程简化为式(4)。

式中:k 为比例系数，其与相机到标定板之间的距离成正比;M 为光敏设备的映射矩阵。将式(4)中的比例系数k 消掉，得到式(5)。

所研发的基于PSD 的测量装置是通过2 套PSD以固定的相对位置组合形成，左右PSD 传感器同时探测空间同一个目标光源，两PSD 输出值与空间坐标可表示为式(6)。

通过最小二乘法求解式(6)，得到目标光源中心在世界坐标系下的坐标值。即为PSD 位置测量工作原理。

2 基于双PSD 的运动轨迹跟踪测量系统研发

2.1 系统硬件搭建

所研发的基于双PSD 的机械手运动轨迹测量装置是由2 个PSD 模块(日本滨松光子学株式会社生产的C10443 -03 型枕型PSD，其有效感应面积为12 mm×12 mm，分辨率为1.4 μm，敏感光谱范围为320～1060 nm，光谱峰值对应波长为920 nm，最大光功率为167 μW，光灵敏度为60 mV/μW)、工业镜头及连接支架以固定的空间夹角组合而成，主要用于测量目标光源的空间位置，如图3 所示。

2.2 信号采集与传输

PSD 测量装置的信号传输和数据采集如图4 所示。C10443 -03 型PSD模块作为传感器检测目标光源的光信号，在PSD 内部实现光信号到位置信号的转换，在反偏电压的驱动下，PSD 将位置信号以电压量输出。C10460 型信号处理器给PSD 模块提供反偏电压，同时将PSD 模块输出的电压量调整为与光斑位置以1 mm/V 比例对应。NI 9215 模拟电压采集卡采集C10460 型信号处理器输出的电压信号，并以最高100 kHz 的采样频率对电压信号采用后传送给工控机进行计算。

PSD 测量装置的测量目标是一个稳定的散射点光源，要想测量机械手的角位移和轴向位移，必须将被测目标光源放置在机械手臂上。由于ATC 的复杂结构和特殊的运动过程，所以，目标光源必须进行恒流驱动和无线控制。图5 为目标光源的无线控制和恒流驱动流程图。为了能够提供足够的光功率，目标光源选择电光转换效率高的大功率LED 点光源。使用锂电池和恒流驱动电路给LED 供电，保证了光源的稳定性和使用寿命。驱动电路的工作状态由无线数字传输模块控制。大容量锂电池组为恒流驱动电路和无线数字传输模块提供电源。为了便于在机械手臂上安装使用，将锂电池组、无线数字传输模块、恒流驱动电路和LED 光源封装在一起，构成目标光源模块，并配备强磁座。在工控机端是光源控制模块，计算机通过NI 9401 数字量采集卡向另一块无线数字传输模块发送控制命令，两个无线数字传输模块通过无线通讯协议相互通信。这种光源控制和驱动方式具有响应速度快、状态切换过程时间短、光源稳定性好及续航时间长等优点。

2.3 系统的标定

PSD 在实际使用前要进行标定。视觉测量设备的标定方法很多，例如使用二维标定板的张氏标定法。PSD 测量装置是用于测量目标光源的三维坐标值，考虑到点光源很难形成精度较高的平面标定模板，所以我们采用三维立体标定法来标定PSD。

根据式(6)，线性投影模型的映射模型可以表示为式(7):

式(7)实质上是3 个相关的方程，将第3 个方程代入前两个得到式(8):

式中:(XW，YW，ZW)表示目标光源中心在世界坐标系中的三维坐标;(u，v)表示目标光源中心映射在PSD感应屏上的光斑在PSD 坐标系下的坐标值，mi是投影矩阵M 的元素。在三维标定中，目标光源的每个测量位置的三维坐标都与投影光斑在PSD 坐标系上的一对二维坐标值形成两个线性方程。当标定实验选择n个测量位置时，就形成2n 个关于投影矩阵M 的方程组(9):

本文利用高精度的三坐标测量机对PSD 测量装置进行三维标定。目标光源采用5 W 大功率LED 灯珠，光源波长为650 nm，采用恒流电路驱动，将光源放置在三坐标测量立柱上，调节PSD 测量装置的位置，保证其测量视野覆盖预设的三坐标机的活动范围，PSD 输出的信号经过PSD 信号处理器、电压数据采集卡，传输到工控机，通过PSD 测量装置标定程序控制标定过程和记录实验数据。实验过程在250 mm×250 mm×170 mm 的三维空间内进行标定，总共平均选取了108 个测量位置点进行标定。将PSD 的测量坐标作为输入函数，三坐标测量机的空间坐标为输出函数，用最小二乘法拟合。然后，在整个测量空间内再随机取27 个位置点，通过对比系统测量数据与三坐标输出的真实位置数据进行对比，对系统误差进行评定。

将108 个测量位置光源的三维坐标和PSD 输出的二维坐标代入式(9)，通过求解方程及优化参数得到两个PSD 设备的坐标转换矩阵Ml和Mr:

PSD 测量装置标定完成之后，就可以进行目标光源三维坐标测量。将标定实验得到的转换矩阵ML和Mr代入式(6)中，计算出目标光源的三维坐标值。

最后，通过27 组实验数据，验证计算得到所搭建的双PSD 测量系统误差绝对值最大值为9.0438 mm;误差绝对值平均值为3.8008 mm。

3 加工中心ATC 角位移和轴向位移测试试验

在机械手角位移和轴向位移测试中，为了直观地显示出机械手实时的角位移和轴向位移变化，建立一个机械手坐标系(如图6 所示)，旋转轴线为Z 轴，旋转平面为XOY 平面，机械手抓刀位置为角度计算零点，此时手臂所指方向为与X 轴平行。

ATC 机械手角位移测量结果，如图7 所示。从凸轮方向观测机械手，其角位移逆时针增长。机械手在刀库方向的抓刀位置为角位移的0°，待机时所处的位置为90°。从角位移图中可以看出，机械手的角位移输出曲线整体平稳，机械手的原始启动位置与抓刀位置之间的角位移并非90°，偏差在8°左右，这可能是装配误差造成的。

ATC 机械手轴向位移的测量结果，如图8 所示。机械手待机时所处的位置为轴向位移零点，沿旋转轴向锥齿轮副方向增长。从轴向位移图上可以看出，机械手在换刀过程中的抖动特别明显，特别是在机械手臂旋转的过程中，在轴向的窜动很大，同时“抓刀”和“回位”动作比“换刀”动作抖动明显。结合角位移和轴向位移可以看出，在“换刀”过程中，机械手臂存在轴向移动现象，使轴向位移峰值段呈现上升趋势。

另外，从两幅测量图中可以看出机械手一个换刀动作用时为2.806 s，由此可见，该种方法还可以测量ATC 的换刀时间。

4 结语

机械手的运动是旋转运动和直线运动的合成，利用非接触式测量仪器对机械手的空间运动进行测量，最后分解为角位移和轴向位移是一种有效的测量方法。非接触式测量设备安装方便、不干扰被测量机构的正常运行，移植性强，适合机械手的角位移和轴向位移测量。目前，利用双PSD 系统进行空间点的非接触轨迹测量方面，国内处于起步阶段，大多采用激光作为目标光源。在工程实际中，很多机械手在三维空间内运动，受限于运动空间，无法安装激光发射器。近红外区是PSD 最敏感的范围，大约在900 nm，因此，能够实现恒流驱动和无线控制的大功率红外LED 可以作为目标光靶，其不会对待测机械臂产生影响。本文探索了一种以LED 光源为目标光源的基于双PSD 的空间点三维运动轨迹跟踪测量系统，测量误差绝对值最大值为9.0438 mm。本文所做研究为解决有限空间内，复杂机构上点目标的运动轨迹实时在线测量、机器人末端执行器的空间位置及姿态的快速测量问题探索了一条可行之路。提高双PSD 系统测量精度对于双PSD 测量系统在机械手运动轨迹跟踪测量的应用方面是重中之重，亟待进行深入研究。

［1］Zhang Liping，Ma Shugen，Li Bin，et al.Two-dimensional PSD based automatic docking of self-reconfiguration modular exploration robot system［J］.Chinese Journal of Scientific Instrument，2007，28(7):1198-1204.

［2］Jing Yuan，Yu S L.End -effector position -orientation measurement［J］.IEEE Transactions on Robotics and Automation，1999，15(3):592 -595.

［3］岁波，都东，陈强，等.基于双目视觉的工业机器人运动轨迹准确度检测［J］.机械工程学报，2003，39(5):87 -91.

［4］Lau K，Hocken R，Haynes L.Robot performance measurements using automatic laser tracking techniques［J］.Robotics and Computer -Integrated Manufacturing，1985，2(3 -4):227 -236.

［5］Spiess S，Vincze M，Ayromlou M.On the calibration of a 6 -D laser tracking system for dynamic robot measurements［J］.IEEE Transactions on Instrumentation and Measurement，1998，47(1):270 -274.

［6］刘永东，王佳，胡朝晖，等.距离法运动目标激光跟踪测量系统的研究［J］.光学仪器.2001，23(3):12 -16.

［7］Hsiao H，Keyserling W M.A three -dimensional ultrasonic system for posture measurement［J］.Ergonomics，1990，33(9):1089 -1114.

［8］吴晓峰，张国雄.室内GPS 测量系统及其在飞机装配中应用［J］.航空精密制造技术.2006，42(5):1 -5.

［9］Schmitt Robert，Schönberg，Alexander，Damm，Björn.Indoor -gps based robots as a key technology for versatile production［C］.Joint 41st International Symposium on Robotics and 6th German Conference on Robotics 2010，ISR/ROBOTIK 2010，1:199 -205.

［10］林永涛.PSD 综合性能检测系统的研制［D］.杭州:浙江大学.2012.

［11］蔡喜平，赵远，戴永江，等.空间飞行器对接的PSD 近程操作模拟器［J］.光电工程，1996，23(6):30 -33.

［12］邓敢，朱建，王兴松.挠性臂挠性偏差及其末端位移的测量［J］.机器人技术与应用，1999(6):24 -26.

［13］张晓芳，王宝光，蒋诚志.位置探测器PSD 应用于三维坐标测量的理论研究［J］.光学技术，2001，27(4):362 -364，366.

［14］蒋丽雁，李立群.基于PSD 的激光微位移测量系统［J］.工具技术，2013(4):70 -72.

［15］陆军，宋景豪，刘永.基于视觉和PSD 的高精度机械手伺服控制［C］.第三十二届中国控制会议论文集(D 卷)，2013(7):5650 -5655.

［16］李兴达.基于PSD 的光点空间位置测量技术研究［D］.天津:天津大学，2012.