基于雙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.