基于PSD的并联微定位平台位姿检测方法

荆龙宾，梁 康，黄山云，闫军伟，夏 刚

（1.北京航天控制仪器研究所，北京100039；2.中国人民解放军国防大学联合勤务学院，北京100039；3.中国航天电子技术研究院，北京100094）

0 引言

并联机构是国际上机械装备研究领域的热点课题，它是知识密集型和技术密集型高度结合的机电一体化高科技产品，是并联运动机构学、计算机学、控制理论、精密测量、多体系统等多学科理论研究的交叉领域。并联机构除了具有承载能力强、刚度高、动态响应特性好的优点外，还有一个突出的特点就是具有很高的机械综合精度和控制精度，其在并联运动机床、并联机器人、定位装置、测量装置、飞行模拟器、医疗、娱乐等诸多领域得到了广泛的应用。

研究表明制造和装配引起的几何误差是造成并联机构末端位姿误差的重要原因［1］，如何对平台进行位姿检测以进行精度标定是研究并联平台的一项重要内容。Chiu等［2］利用安装在静平台和动平台之间的可伸缩杆，实现了对Hexapod并联机床的精度检测。邵珠峰等［3］借助静平台上的标准定位圆孔，通过仪器对拉线式编码器进行标定，进而利用线尺在线测量、记录机构中的实际位姿，结合数控系统中的理论轨迹，实现了3⁃RRR并联机构的参数辨识。上述方法均为自标定的检测方法，自检测方法的不足在于有些机构的被动铰链安装传感器时较为困难甚至是不可能的［4］。外部检测法利用外部传感器检测末端位姿信息，并构造它们与模型计算值之间的残差，进而通过相应的逆解或者正解辨识模型来识别几何参数。Rauf等［5］通过使用一套包括双轴倾角传感器、线性可变差动变压换能器以及旋转传感器的设备为测量手段对六自由度的HexaSlide机构进行了精度检测研究，使其精度得到了大幅度提高，但鲁棒性相对较差。Traslosheros等［6］以计算机视觉为测量手段对并联机构的精度检测进行了研究，经济性更强，但这类方法受视觉系统测量精度的限制，对最终的精度检测结果有较大的影响。罗继曼等［7］提出了一种利用激光检测仪和最小二乘算法对并联机器人进行精度标定的研究方法，精度较高，但结构复杂，经济性较差。

本文以六自由度并联微定位平台为研究对象，基于二维PSD位置传感器提出一种非接触式光学检测方法，建立了位姿检测的理论模型，在此基础上设计了一套位姿检测系统。仿真分析验证了理论模型的正确性，同时用该方法进行了误差分析。结果表明，本文设计的这套检测系统能实时在线测量平台位姿，并且集成在定位平台里，具有体积小、结构简单可靠、可操作性强等优点。

1 二维PSD光电位置传感器

PSD（Position Sensitive Detector）位置探测器是利用半导体横向光电效应实现对入射光点位置进行检测的一种光电器件，与传统的象限电池、CCD相比，PSD具有位置分辨率高、响应速度快、不需要扫描、处理电路简单等优点。此外，还由于PSD是非分割型元件，可连续输出模拟信号，无死区，位置输出信号只与入射光的中心位置有关，对光斑形状无严格要求，因而光学系统比较简单。二维PSD工作原理如图1所示。

从图1可知，通过表面的p型电阻层检测X方向的位置，通过里面的n型电阻层检测Y方向的位置。这时，由于n＋层内的电荷分布不匀形成电流，用此电流进行检测，这时电流方向是流向PSD内［8］，光点的入射位置与电流之间有如下关系：

式中，I0为总电流，I0＝Ix1＋Ix2＝Iy1＋Iy2；Ix1为X方向电极1的电流；Ix2为X方向电极2的电流；Iy1为Y方向电极1的电流；Iy2为Y方向电极2的电流；Lx为X方向电极间（受光面）长度；Ly为Y方向电极间（受光面）长度；x是坐标轴以受光面为中心时光入射位置的X坐标；y是坐标轴以受光面为中心时光入射位置的Y坐标。

可求出光入射位置坐标：

2 位姿检测系统理论模型的建立

图2所示为六自由度微定位平台模型，平台结构由上下平台及6根电动缸支杆组成。其中，下平台为定平台，上平台为动平台，电动缸与上下平台之间分别由6个球铰连接。伺服电机驱动6根电动缸的伸缩达到精确控制上平台位姿，实现上平台的定位目的。

六自由度微定位平台的结构参数如图3所示。图3中，ra为上绞圆半径，rb为下绞圆半径，α为上绞点与中心连线与坐标轴夹角，β为上绞点与中心连线与坐标轴夹角，H为平台高度，h为质心高度。

在此平台基础上建立位姿检测系统，系统理论模型如图4所示，为便于观察和计算，将其放入立体三维坐标系里。在图4中，A点为激光原点，与上平台固连，①②③分别为3个二维PSD探测器，放置于与下平台固连在一起并平行于下平台的工装板上，3个探测器组成边长为t的正三角形，激光原点到探测器平面的距离为l，A在探测器平面上的初始零位投影点为坐标系原点O。当动平台沿X轴、Y轴、Z轴平移及绕X轴、Y轴、Z轴转动时，激光点在3个PSD探测器上有相应的位移量，建立6个位移量与6个自由度的变化量之间的关系式，反解出动平台的位移及转动量，现分别求动平台沿X轴、Y轴、Z轴平移及绕X轴、Y轴、Z轴转动时PSD探测器上相对应的位移量。设3个PSD探测器上X、Y方向上的位移量分别为ΔX1、 ΔY1、 ΔX2、 ΔY2、 ΔX3、 ΔY3。

（1）动平台沿X轴平移a个位移

（2）动平台沿Y轴平移b个位移

（3）动平台沿Z轴平移c个位移

（4）动平台绕X轴转动α个单位（方向遵循右手法则）

动平台绕X轴转动1周，与②号PSD相对应的激光线形成的锥面方程为：

与平面z＝0联立解为：

可得：

将式（14）代入（13）得：

（5）动平台绕Y轴转动β个单位

同理，可得如下公式：

（6）动平台绕Z轴转动γ个单位

将上述六组（ΔX1， ΔY1， ΔX2， ΔY2， ΔX3，ΔY3）分别相加，即为动平台沿X轴、Y轴、Z轴平移及绕X轴、Y轴、Z轴转动时PSD探测器上相对应的位移量，得到未知数为a、b、c、α、β、γ的一组非线性方程组。

3 理论模型验证

此处运用拟牛顿法（Broyden 方法）［9］求解非线性方程组。牛顿法每迭代一次，就要计算当前一步的Jacobi矩阵的逆矩阵，计算量比较大。为了解决逆矩阵问题，设法构造一个矩阵，逼近逆矩阵，即为拟牛顿法。Broyden方法基本迭代格式为：

其中， Δxk＝xk＋1－xk，yk＝f（xk＋1）－f（xk）。

在Matlab中，运用Broyden方法求解上节的非线性方程组。设定迭代次数为2000次，有针对性地选取几个有代表性的位姿坐标，并求解计算值与理论值之差，结果如表1所示。

由表1结果可知，在有限次的迭代计算中，选取的位置姿态坐标计算值与理论值之差小于10－5mm，验证了理论模型的解算是正确和可行的，且具备较高的迭代精度。

表1 计算结果Table 1 Results of calculation

4 误差分析

4.1 误差源

本文研究的六自由度微定位平台的位姿测量误差的来源主要有3个方面：1）位姿检测系统加工装配误差（含工装加工尺寸误差、安装调试空间位置误差）；2）激光源照射误差（含光斑形状、光斑抖动误差）；3）PSD位置传感器位置测量误差（含PSD测量精度、电噪声误差）。其中，加工装配误差在对PSD做初始化的时候进行补偿，故影响测量精度的误差主要是激光源照射和PSD位置传感器测量误差。

4.2 误差仿真分析

在Matlab软件内，PSD测量值叠加一幅值为0.01mm的随机噪声，计算此时二维PSD存在测量噪声时解算的实时位姿，并与理论位姿进行比较。选取4个典型位姿进行分析，结果如图6所示。

进一步对100次误差迭代结果取均方根值，结果如表2所示。

表2 6个自由度误差均方根值Table 2 Root mean square value of six degrees

从图6及表2可以看出，PSD位置传感器测量误差导致的平移误差在0.01mm之内，转动误差在0.002°之内。结果表明，该系统能够快速准确解算六自由度定位平台位姿，同时具备较高的测量精度。

5 结论

本文基于PSD位置传感器设计了六自由度光学位姿检测系统，在理论分析的基础上建立了姿态解算的数学模型，并做了仿真分析，分析结果验证了姿态解算模型的正确性。针对姿态解算模型，进一步进行了误差分析。分析结果表明，本文设计的光学检测系统能同时解算并联平台的6个位姿，同时具备较高的测量精度，平移误差在0.01mm之内，转动误差在0.002°之内。