基于PSD的并聯(lián)微定位平臺(tái)位姿檢測(cè)方法

荊龍賓，梁 康，黃山云，閆軍偉，夏 剛

（1.北京航天控制儀器研究所，北京100039；2.中國人民解放軍國防大學(xué)聯(lián)合勤務(wù)學(xué)院，北京100039；3.中國航天電子技術(shù)研究院，北京100094）

0 引言

并聯(lián)機(jī)構(gòu)是國際上機(jī)械裝備研究領(lǐng)域的熱點(diǎn)課題，它是知識(shí)密集型和技術(shù)密集型高度結(jié)合的機(jī)電一體化高科技產(chǎn)品，是并聯(lián)運(yùn)動(dòng)機(jī)構(gòu)學(xué)、計(jì)算機(jī)學(xué)、控制理論、精密測(cè)量、多體系統(tǒng)等多學(xué)科理論研究的交叉領(lǐng)域。并聯(lián)機(jī)構(gòu)除了具有承載能力強(qiáng)、剛度高、動(dòng)態(tài)響應(yīng)特性好的優(yōu)點(diǎn)外，還有一個(gè)突出的特點(diǎn)就是具有很高的機(jī)械綜合精度和控制精度，其在并聯(lián)運(yùn)動(dòng)機(jī)床、并聯(lián)機(jī)器人、定位裝置、測(cè)量裝置、飛行模擬器、醫(yī)療、娛樂等諸多領(lǐng)域得到了廣泛的應(yīng)用。

研究表明制造和裝配引起的幾何誤差是造成并聯(lián)機(jī)構(gòu)末端位姿誤差的重要原因［1］，如何對(duì)平臺(tái)進(jìn)行位姿檢測(cè)以進(jìn)行精度標(biāo)定是研究并聯(lián)平臺(tái)的一項(xiàng)重要內(nèi)容。Chiu等［2］利用安裝在靜平臺(tái)和動(dòng)平臺(tái)之間的可伸縮桿，實(shí)現(xiàn)了對(duì)Hexapod并聯(lián)機(jī)床的精度檢測(cè)。邵珠峰等［3］借助靜平臺(tái)上的標(biāo)準(zhǔn)定位圓孔，通過儀器對(duì)拉線式編碼器進(jìn)行標(biāo)定，進(jìn)而利用線尺在線測(cè)量、記錄機(jī)構(gòu)中的實(shí)際位姿，結(jié)合數(shù)控系統(tǒng)中的理論軌跡，實(shí)現(xiàn)了3?RRR并聯(lián)機(jī)構(gòu)的參數(shù)辨識(shí)。上述方法均為自標(biāo)定的檢測(cè)方法，自檢測(cè)方法的不足在于有些機(jī)構(gòu)的被動(dòng)鉸鏈安裝傳感器時(shí)較為困難甚至是不可能的［4］。外部檢測(cè)法利用外部傳感器檢測(cè)末端位姿信息，并構(gòu)造它們與模型計(jì)算值之間的殘差，進(jìn)而通過相應(yīng)的逆解或者正解辨識(shí)模型來識(shí)別幾何參數(shù)。Rauf等［5］通過使用一套包括雙軸傾角傳感器、線性可變差動(dòng)變壓換能器以及旋轉(zhuǎn)傳感器的設(shè)備為測(cè)量手段對(duì)六自由度的HexaSlide機(jī)構(gòu)進(jìn)行了精度檢測(cè)研究，使其精度得到了大幅度提高，但魯棒性相對(duì)較差。Traslosheros等［6］以計(jì)算機(jī)視覺為測(cè)量手段對(duì)并聯(lián)機(jī)構(gòu)的精度檢測(cè)進(jìn)行了研究，經(jīng)濟(jì)性更強(qiáng)，但這類方法受視覺系統(tǒng)測(cè)量精度的限制，對(duì)最終的精度檢測(cè)結(jié)果有較大的影響。羅繼曼等［7］提出了一種利用激光檢測(cè)儀和最小二乘算法對(duì)并聯(lián)機(jī)器人進(jìn)行精度標(biāo)定的研究方法，精度較高，但結(jié)構(gòu)復(fù)雜，經(jīng)濟(jì)性較差。

本文以六自由度并聯(lián)微定位平臺(tái)為研究對(duì)象，基于二維PSD位置傳感器提出一種非接觸式光學(xué)檢測(cè)方法，建立了位姿檢測(cè)的理論模型，在此基礎(chǔ)上設(shè)計(jì)了一套位姿檢測(cè)系統(tǒng)。仿真分析驗(yàn)證了理論模型的正確性，同時(shí)用該方法進(jìn)行了誤差分析。結(jié)果表明，本文設(shè)計(jì)的這套檢測(cè)系統(tǒng)能實(shí)時(shí)在線測(cè)量平臺(tái)位姿，并且集成在定位平臺(tái)里，具有體積小、結(jié)構(gòu)簡(jiǎn)單可靠、可操作性強(qiáng)等優(yōu)點(diǎn)。

1 二維PSD光電位置傳感器

PSD（Position Sensitive Detector）位置探測(cè)器是利用半導(dǎo)體橫向光電效應(yīng)實(shí)現(xiàn)對(duì)入射光點(diǎn)位置進(jìn)行檢測(cè)的一種光電器件，與傳統(tǒng)的象限電池、CCD相比，PSD具有位置分辨率高、響應(yīng)速度快、不需要掃描、處理電路簡(jiǎn)單等優(yōu)點(diǎn)。此外，還由于PSD是非分割型元件，可連續(xù)輸出模擬信號(hào)，無死區(qū)，位置輸出信號(hào)只與入射光的中心位置有關(guān)，對(duì)光斑形狀無嚴(yán)格要求，因而光學(xué)系統(tǒng)比較簡(jiǎn)單。二維PSD工作原理如圖1所示。

從圖1可知，通過表面的p型電阻層檢測(cè)X方向的位置，通過里面的n型電阻層檢測(cè)Y方向的位置。這時(shí)，由于n＋層內(nèi)的電荷分布不勻形成電流，用此電流進(jìn)行檢測(cè)，這時(shí)電流方向是流向PSD內(nèi)［8］，光點(diǎn)的入射位置與電流之間有如下關(guān)系：

式中，I0為總電流，I0＝Ix1＋I(xiàn)x2＝Iy1＋I(xiàn)y2；Ix1為X方向電極1的電流；Ix2為X方向電極2的電流；Iy1為Y方向電極1的電流；Iy2為Y方向電極2的電流；Lx為X方向電極間（受光面）長(zhǎng)度；Ly為Y方向電極間（受光面）長(zhǎng)度；x是坐標(biāo)軸以受光面為中心時(shí)光入射位置的X坐標(biāo)；y是坐標(biāo)軸以受光面為中心時(shí)光入射位置的Y坐標(biāo)。

可求出光入射位置坐標(biāo)：

2 位姿檢測(cè)系統(tǒng)理論模型的建立

圖2所示為六自由度微定位平臺(tái)模型，平臺(tái)結(jié)構(gòu)由上下平臺(tái)及6根電動(dòng)缸支桿組成。其中，下平臺(tái)為定平臺(tái)，上平臺(tái)為動(dòng)平臺(tái)，電動(dòng)缸與上下平臺(tái)之間分別由6個(gè)球鉸連接。伺服電機(jī)驅(qū)動(dòng)6根電動(dòng)缸的伸縮達(dá)到精確控制上平臺(tái)位姿，實(shí)現(xiàn)上平臺(tái)的定位目的。

六自由度微定位平臺(tái)的結(jié)構(gòu)參數(shù)如圖3所示。圖3中，ra為上絞圓半徑，rb為下絞圓半徑，α為上絞點(diǎn)與中心連線與坐標(biāo)軸夾角，β為上絞點(diǎn)與中心連線與坐標(biāo)軸夾角，H為平臺(tái)高度，h為質(zhì)心高度。

在此平臺(tái)基礎(chǔ)上建立位姿檢測(cè)系統(tǒng)，系統(tǒng)理論模型如圖4所示，為便于觀察和計(jì)算，將其放入立體三維坐標(biāo)系里。在圖4中，A點(diǎn)為激光原點(diǎn)，與上平臺(tái)固連，①②③分別為3個(gè)二維PSD探測(cè)器，放置于與下平臺(tái)固連在一起并平行于下平臺(tái)的工裝板上，3個(gè)探測(cè)器組成邊長(zhǎng)為t的正三角形，激光原點(diǎn)到探測(cè)器平面的距離為l，A在探測(cè)器平面上的初始零位投影點(diǎn)為坐標(biāo)系原點(diǎn)O。當(dāng)動(dòng)平臺(tái)沿X軸、Y軸、Z軸平移及繞X軸、Y軸、Z軸轉(zhuǎn)動(dòng)時(shí)，激光點(diǎn)在3個(gè)PSD探測(cè)器上有相應(yīng)的位移量，建立6個(gè)位移量與6個(gè)自由度的變化量之間的關(guān)系式，反解出動(dòng)平臺(tái)的位移及轉(zhuǎn)動(dòng)量，現(xiàn)分別求動(dòng)平臺(tái)沿X軸、Y軸、Z軸平移及繞X軸、Y軸、Z軸轉(zhuǎn)動(dòng)時(shí)PSD探測(cè)器上相對(duì)應(yīng)的位移量。設(shè)3個(gè)PSD探測(cè)器上X、Y方向上的位移量分別為ΔX1、 ΔY1、 ΔX2、 ΔY2、 ΔX3、 ΔY3。

（1）動(dòng)平臺(tái)沿X軸平移a個(gè)位移

（2）動(dòng)平臺(tái)沿Y軸平移b個(gè)位移

（3）動(dòng)平臺(tái)沿Z軸平移c個(gè)位移

（4）動(dòng)平臺(tái)繞X軸轉(zhuǎn)動(dòng)α個(gè)單位（方向遵循右手法則）

動(dòng)平臺(tái)繞X軸轉(zhuǎn)動(dòng)1周，與②號(hào)PSD相對(duì)應(yīng)的激光線形成的錐面方程為：

與平面z＝0聯(lián)立解為：

可得：

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

（5）動(dòng)平臺(tái)繞Y軸轉(zhuǎn)動(dòng)β個(gè)單位

同理，可得如下公式：

（6）動(dòng)平臺(tái)繞Z軸轉(zhuǎn)動(dòng)γ個(gè)單位

將上述六組（ΔX1， ΔY1， ΔX2， ΔY2， ΔX3，ΔY3）分別相加，即為動(dòng)平臺(tái)沿X軸、Y軸、Z軸平移及繞X軸、Y軸、Z軸轉(zhuǎn)動(dòng)時(shí)PSD探測(cè)器上相對(duì)應(yīng)的位移量，得到未知數(shù)為a、b、c、α、β、γ的一組非線性方程組。

3 理論模型驗(yàn)證

此處運(yùn)用擬牛頓法（Broyden 方法）［9］求解非線性方程組。牛頓法每迭代一次，就要計(jì)算當(dāng)前一步的Jacobi矩陣的逆矩陣，計(jì)算量比較大。為了解決逆矩陣問題，設(shè)法構(gòu)造一個(gè)矩陣，逼近逆矩陣，即為擬牛頓法。Broyden方法基本迭代格式為：

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

在Matlab中，運(yùn)用Broyden方法求解上節(jié)的非線性方程組。設(shè)定迭代次數(shù)為2000次，有針對(duì)性地選取幾個(gè)有代表性的位姿坐標(biāo)，并求解計(jì)算值與理論值之差，結(jié)果如表1所示。

由表1結(jié)果可知，在有限次的迭代計(jì)算中，選取的位置姿態(tài)坐標(biāo)計(jì)算值與理論值之差小于10－5mm，驗(yàn)證了理論模型的解算是正確和可行的，且具備較高的迭代精度。

表1 計(jì)算結(jié)果Table 1 Results of calculation

4 誤差分析

4.1 誤差源

本文研究的六自由度微定位平臺(tái)的位姿測(cè)量誤差的來源主要有3個(gè)方面：1）位姿檢測(cè)系統(tǒng)加工裝配誤差（含工裝加工尺寸誤差、安裝調(diào)試空間位置誤差）；2）激光源照射誤差（含光斑形狀、光斑抖動(dòng)誤差）；3）PSD位置傳感器位置測(cè)量誤差（含PSD測(cè)量精度、電噪聲誤差）。其中，加工裝配誤差在對(duì)PSD做初始化的時(shí)候進(jìn)行補(bǔ)償，故影響測(cè)量精度的誤差主要是激光源照射和PSD位置傳感器測(cè)量誤差。

4.2 誤差仿真分析

在Matlab軟件內(nèi)，PSD測(cè)量值疊加一幅值為0.01mm的隨機(jī)噪聲，計(jì)算此時(shí)二維PSD存在測(cè)量噪聲時(shí)解算的實(shí)時(shí)位姿，并與理論位姿進(jìn)行比較。選取4個(gè)典型位姿進(jìn)行分析，結(jié)果如圖6所示。

進(jìn)一步對(duì)100次誤差迭代結(jié)果取均方根值，結(jié)果如表2所示。

表2 6個(gè)自由度誤差均方根值Table 2 Root mean square value of six degrees

從圖6及表2可以看出，PSD位置傳感器測(cè)量誤差導(dǎo)致的平移誤差在0.01mm之內(nèi)，轉(zhuǎn)動(dòng)誤差在0.002°之內(nèi)。結(jié)果表明，該系統(tǒng)能夠快速準(zhǔn)確解算六自由度定位平臺(tái)位姿，同時(shí)具備較高的測(cè)量精度。

5 結(jié)論

本文基于PSD位置傳感器設(shè)計(jì)了六自由度光學(xué)位姿檢測(cè)系統(tǒng)，在理論分析的基礎(chǔ)上建立了姿態(tài)解算的數(shù)學(xué)模型，并做了仿真分析，分析結(jié)果驗(yàn)證了姿態(tài)解算模型的正確性。針對(duì)姿態(tài)解算模型，進(jìn)一步進(jìn)行了誤差分析。分析結(jié)果表明，本文設(shè)計(jì)的光學(xué)檢測(cè)系統(tǒng)能同時(shí)解算并聯(lián)平臺(tái)的6個(gè)位姿，同時(shí)具備較高的測(cè)量精度，平移誤差在0.01mm之內(nèi)，轉(zhuǎn)動(dòng)誤差在0.002°之內(nèi)。