莫嘉嗣， 陳健歡， 溫遠(yuǎn)航， 梁杰俊， 陳秋爍， 閆國琦

（華南農(nóng)業(yè)大學(xué) 工程學(xué)院，廣東 廣州 510642）

1 引 言

目前，機(jī)器人的運(yùn)動(dòng)控制方式一般可分為開環(huán)控制系統(tǒng)［1］、僅含關(guān)節(jié)反饋的半閉環(huán)控制系統(tǒng)［2］和包含關(guān)節(jié)反饋與末端反饋的全閉環(huán)系統(tǒng)［3］3 種。為了兼顧精度、成本和體積，大部分的機(jī)器人都是僅含驅(qū)動(dòng)關(guān)節(jié)反饋的半閉環(huán)機(jī)構(gòu)。半閉環(huán)機(jī)器人由于末端狀態(tài)信息不能實(shí)時(shí)觀測(cè)，需要進(jìn)一步提高定位精度，這是一項(xiàng)很有挑戰(zhàn)性的工作。

機(jī)器人末端執(zhí)行器的位姿測(cè)量是機(jī)器人實(shí)現(xiàn)標(biāo)定、實(shí)時(shí)反饋的關(guān)鍵技術(shù)，而缺乏合適的多自由度傳感器實(shí)現(xiàn)末端位姿反饋則是機(jī)器人領(lǐng)域的共性問題。在缺乏末端測(cè)量反饋的前提下要提高機(jī)器人的末端定位精度一般采取兩種途徑，一是提高驅(qū)動(dòng)關(guān)節(jié)定位精度，莫嘉嗣等采用自適應(yīng)前饋補(bǔ)償控制算法提高了平面3PRR 并聯(lián)機(jī)器人驅(qū)動(dòng)關(guān)節(jié)軌跡跟蹤定位精度［4］；二是使用標(biāo)定算法對(duì)運(yùn)動(dòng)學(xué)參數(shù)進(jìn)行標(biāo)定，Mo 等采用誤差模型結(jié)合輔助測(cè)量手段的改進(jìn)標(biāo)定算法提高了平面3PRR 并聯(lián)機(jī)器人的末端定位精度［5］。盡管這兩種手段能不同程度地改進(jìn)機(jī)器人末端的定位精度，但是仍然存在不足。一方面，雖然驅(qū)動(dòng)關(guān)節(jié)（一般是單自由度機(jī)器人）有能力實(shí)現(xiàn)高精度定位，但在驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)傳遞到末端的過程中，傳動(dòng)鏈會(huì)引入加工誤差、裝配誤差、彈形變誤差，以及齒輪傳動(dòng)誤差等非線性因素［6］。這些非線性因素是不確定的，會(huì)導(dǎo)致末端定位誤差的產(chǎn)生。這些非線性因素與誤差來源在關(guān)節(jié)運(yùn)動(dòng)傳遞到末端運(yùn)動(dòng)的過程中是不可控的，同時(shí)誤差在運(yùn)動(dòng)鏈中的傳遞又具有不確定性，因此單純依靠提高驅(qū)動(dòng)關(guān)節(jié)的定位精度對(duì)提高末端定位精度的作用是有限的。另一方面，采用標(biāo)定方法雖然能提高末端定位精度，但并不是一種實(shí)時(shí)提高精度的方法，對(duì)于慢時(shí)變系統(tǒng)（例如剛性機(jī)器人），在標(biāo)定后的短時(shí)間內(nèi)是有效的，但不能保證長期有效；假如長時(shí)間工作后出現(xiàn)了磨損，或者由于生產(chǎn)需要而搬運(yùn)機(jī)器人后重新安裝，為了保證精度，必須再次重新標(biāo)定參數(shù)，標(biāo)定過程冗長復(fù)雜，影響工作效率。

為了進(jìn)一步提高速度，降低由于慣性產(chǎn)生的動(dòng)力學(xué)特性的影響，人們開始減輕機(jī)器人的質(zhì)量，桿件越來越輕薄，柔性越來越大，并成為剛?cè)狁詈蠙C(jī)構(gòu)。剛?cè)狁詈蠙C(jī)構(gòu)是快時(shí)變系統(tǒng)，其運(yùn)動(dòng)過程復(fù)雜，充滿不確定性，因此離線標(biāo)定難以補(bǔ)償其末端定位誤差，機(jī)構(gòu)的運(yùn)動(dòng)學(xué)參數(shù)在線實(shí)時(shí)辨識(shí)和末端實(shí)時(shí)反饋是提高剛?cè)狁詈蠙C(jī)構(gòu)性能的有效手段。機(jī)構(gòu)末端多自由度運(yùn)動(dòng)狀態(tài)信息測(cè)量是實(shí)現(xiàn)機(jī)構(gòu)運(yùn)動(dòng)學(xué)參數(shù)在線辨識(shí)、末端實(shí)時(shí)全反饋控制的關(guān)鍵環(huán)節(jié)，是制約機(jī)構(gòu)性能的瓶頸之一，但就目前而言，尚缺乏集成的多自由度測(cè)量手段。

改進(jìn)驅(qū)動(dòng)關(guān)節(jié)控制算法或采用參數(shù)標(biāo)定算法等手段都是間接提高末端精度的手段，但并不能從根本上解決問題。假如機(jī)器人驅(qū)動(dòng)關(guān)節(jié)與機(jī)器人末端多自由度運(yùn)動(dòng)狀態(tài)信息能夠?qū)崟r(shí)測(cè)量反饋，搭建機(jī)器人全閉環(huán)控制系統(tǒng)則可輕易實(shí)現(xiàn)奇異規(guī)避、振動(dòng)抑制和軌跡跟蹤等綜合性控制，對(duì)提高機(jī)器人性能具有重要意義。

目前，比較先進(jìn)的多自由度運(yùn)動(dòng)狀態(tài)信息測(cè)量手段有拉線式位移傳感器［7］、三坐標(biāo)測(cè)量儀［8］和球桿儀［9］等接觸式測(cè)量手段，以及激光跟蹤儀［10］、光學(xué)運(yùn)動(dòng)跟蹤系統(tǒng)［11］和機(jī)器視覺［12］等非接觸測(cè)量手段。接觸式測(cè)量由于妨礙了機(jī)器人的運(yùn)動(dòng)作業(yè)，一般只作為離線標(biāo)定手段，并不用于機(jī)器人實(shí)時(shí)在線反饋。非接觸式測(cè)量中的激光跟蹤儀是目前精度和實(shí)時(shí)性最好的測(cè)量方式，具有測(cè)量精度高、測(cè)量速度快和測(cè)量范圍廣等優(yōu)勢(shì)［13］，但需要在機(jī)器人末端裝載對(duì)靶物，實(shí)際上也影響了機(jī)器人運(yùn)動(dòng)，而且測(cè)量的自由度有限。例如，激光跟蹤儀搭載一個(gè)靶球，只能測(cè)量3 個(gè)平動(dòng)自由度，若要測(cè)量姿態(tài)，需要兩臺(tái)激光跟蹤儀同時(shí)工作或一臺(tái)跟蹤儀通過多個(gè)靶球分時(shí)解算［14］。天津大學(xué)在機(jī)器人末端固定4 個(gè)非共線的參考點(diǎn)，均能被激光跟蹤儀測(cè)量，間接得出機(jī)器人末端的位姿［15］。此外，激光跟蹤儀十分昂貴，并不適合長時(shí)間在生產(chǎn)現(xiàn)場(chǎng)對(duì)機(jī)器人進(jìn)行實(shí)時(shí)反饋控制，一般只用于定期標(biāo)定［16］。

機(jī)器視覺成本較低，使用靈活，常用于機(jī)器人末端測(cè)量。平面測(cè)量常用單目視覺，Li 等開發(fā)了一種單目視覺系統(tǒng)，用于在線測(cè)量3PRR 平面并聯(lián)機(jī)器人的位姿［17］。但是單目視覺對(duì)焦距有一定的要求，難以集成到需要改變Z軸高度的運(yùn)動(dòng)機(jī)器人測(cè)量上［18］；而且單目視覺在測(cè)量姿態(tài)上比較困難，往往需要輔以別的傳感器，張?zhí)祺雽⒆藨B(tài)傳感器和視覺傳感器結(jié)合，以低成本的方式測(cè)量末端位姿，提高了測(cè)量精度［19］。機(jī)器視覺的測(cè)量速度與被測(cè)物體的復(fù)雜程度和相機(jī)的像素大小掛鉤，測(cè)量復(fù)雜物體時(shí)，算法耗時(shí)較長，則響應(yīng)速率不夠；如果為了提高檢測(cè)精度而提高像素，則數(shù)據(jù)傳輸量增大，響應(yīng)速率降低；提高像素也會(huì)帶來視場(chǎng)大小與檢測(cè)高度的變化，使得機(jī)器視覺測(cè)量受限；在三維測(cè)量上，需要用多目視覺，多個(gè)相機(jī)安裝在機(jī)器人上，其體積和質(zhì)量勢(shì)必會(huì)影響機(jī)器人的正常工作，而多目相機(jī)的標(biāo)定也是目前的研究難點(diǎn)。盡管傳統(tǒng)視覺位姿估計(jì)方法比較成熟，但是它特別依賴先驗(yàn)知識(shí)，自適應(yīng)性和泛化性較差［20］。

綜上所述，目前機(jī)器人末端測(cè)量反饋除了精度、實(shí)時(shí)性等基本測(cè)量要求外，還要求不增加太多的輔助結(jié)構(gòu)，不影響機(jī)器人的正常工作。

多源傳感測(cè)量通過搭建多傳感器系統(tǒng)，對(duì)多傳感器信息進(jìn)行融合估計(jì)實(shí)現(xiàn)測(cè)量［21］，可采用多種低成本傳感器實(shí)現(xiàn)較高精度的測(cè)量，能較好地解決機(jī)器人末端多自由度測(cè)量反饋問題。Guo等使用3 個(gè)激光位移傳感器融合，解算出柔順并聯(lián)平臺(tái)的末端位姿后進(jìn)行反饋［22］。本文提出一種采用多光學(xué)位置傳感器（Position Sensitive Detector，PSD）融合的平面機(jī)器人3 自由度非接觸測(cè)量裝置與方法，在只需搭載十字線激光的前提下，可實(shí)現(xiàn)平面3 自由度機(jī)器人的高速、高精度測(cè)量和反饋，并且不受Z軸高度的限制，易于集成到更多自由度的測(cè)量系統(tǒng)中。

2 原 理

2.1 一維PSD 傳感器測(cè)量原理

本文提出4-PSD 測(cè)量裝置，采用4 個(gè)一維PSD 通過合理的位置擺放建立測(cè)量模型，實(shí)現(xiàn)平面3 自由度非接觸測(cè)量的目的。該裝置選用日本濱松公司的一維PSD 傳感器，感光面積為2.5 mm×70 mm，光譜響應(yīng)為400~1 100 nm，線性誤差為0.3%，分辨率為10 μm。如圖1 所示，一維PSD 傳感器是P-I-N 結(jié)構(gòu)，實(shí)際上是利用橫向光電效應(yīng)，采用連續(xù)的平面擴(kuò)散型光電二極管，沒有條帶或盲區(qū)，能測(cè)量出一個(gè)光斑在它的感應(yīng)面上的位置。

圖1 一維PSD 傳感器位置測(cè)量原理Fig.1 Measurement principle of one-dimensional PSD sensor

以感光面積的幾何中心為原點(diǎn)，當(dāng)有入射光照入到傳感器的有效感光區(qū)域時(shí)，會(huì)產(chǎn)生二條通道的感應(yīng)電流I1和I2，其測(cè)量動(dòng)態(tài)范圍寬，輸出為模擬信號(hào)，因此，測(cè)量頻率完全由處理電路的A/DC 采樣頻率決定，其值很高。2 條通道輸出的感應(yīng)電流與傳感器的光斑位置關(guān)系為：

只要測(cè)得感應(yīng)電流I1和I2就可以進(jìn)一步計(jì)算得到光斑位置x。該傳感器能測(cè)量到邊緣的光斑位置，并且入射光斑形狀、密度分布對(duì)測(cè)量位置讀數(shù)基本沒有影響，輸入光斑可以是任意的尺寸和形狀。這是因?yàn)檩敵鲂盘?hào)由光斑中心的幾何位置決定，即輸出信號(hào)與光斑到PSD 感光區(qū)域中心的位移量成正比，為后續(xù)使用十字線激光（650 nm 紅光）進(jìn)行測(cè)量提供便利。

2.2 測(cè)量模型推導(dǎo)

通過一維PSD 傳感器融合測(cè)量平面（x，y）自由度，首先采用正交（90°）十字線激光器對(duì)兩個(gè)正交分布的PSD 傳感器進(jìn)行平移測(cè)量，但若十字線激光器光線與PSD 感光軸線不垂直（見圖2），測(cè)量出的（x，y）會(huì)產(chǎn)生偏差，其原因是缺少繞Z軸旋轉(zhuǎn)的α角度從而引入了誤差。為了測(cè)量α角，可對(duì)稱地放置4 個(gè)一維PSD 傳感器，通過正交放置，配合十字線激光器，可以實(shí)現(xiàn)平面3 自由度（x，y，α）的非接觸式測(cè)量，測(cè)量原理如圖3所示。

圖2 光斑一維坐標(biāo)與4-PSD 坐標(biāo)系分布Fig.2 Distribution of one-dimensional coordinates of light spot and 4-PSD sensor coordinate system

圖3 4-PSD 融合測(cè)量原理示意圖Fig.3 Principle diagram for 4-PSD fusion measurement

4-PSD 裝置定平臺(tái)固定部分由4 個(gè)一維PSD傳感器首尾相接，正交（90°放置）分布在傳感器安裝底板四周。機(jī)器人末端搭載十字線激光器，當(dāng)機(jī)器人運(yùn)動(dòng)時(shí)，末端激光線將會(huì)與定平臺(tái)上各PSD 傳感器有相對(duì)位置變化。以定平臺(tái)安裝底板的幾何中心為坐標(biāo)原點(diǎn)O建立坐標(biāo)系，如圖3所示。

如圖2 所示，X軸與PSD2，PSD4平行，Y軸與PSD1，PSD3平行。激光器發(fā)出正交（90°）十字線激光，投影在4-PSD 裝置上。十字線激光的4 段光線分別照射在4 個(gè)PSD 上，其照射光斑產(chǎn)生的感應(yīng)電流代入式（1），計(jì)算得到局部一維坐標(biāo)P1，P2，P3，P4。PSD1上光斑的二維坐標(biāo)為（D+L，-P1），PSD2上為（-P2，-（D+L）），PSD3上為（-（D+L），P3），PSD4上為（P4，D+L）。通過PSD1 和PSD3 感光點(diǎn)的直線為L1，通過PSD2和PSD4 直線為L2。L1的斜率k1=-（P1+P3）/2D′，截距b1=P3-（P1+P3）/2；L2的斜率k2=2D′/（P2+P4），截距b2=D′－2D′P4/（P2+P4）。安裝偏移量為L，L1的方程為y=k1x+b1，L2的方程為y=k2x+b2，聯(lián)立后求解可得交點(diǎn)坐標(biāo)，即激光線中點(diǎn)的（x，y），再通過斜率k1和k2可得α角，即推導(dǎo)出4-PSD 測(cè)量模型如下：

式中：D為PSD 傳感器感光部分長度的一半，k1和k2為激光線斜率，b1和b2為激光線截距，α為末端繞Z軸逆時(shí)針旋轉(zhuǎn)的夾角。

機(jī)器人末端與激光器是固定連接的，激光器體積小，質(zhì)量輕，對(duì)機(jī)器人末端工作幾乎沒有影響。故通過本裝置可以非接觸式地測(cè)量機(jī)器人末端的平移和旋轉(zhuǎn)，共3個(gè)自由度的位姿（x，y，α）。

2.3 一維PSD 信號(hào)處理與采集電路設(shè)計(jì)

式（2）中關(guān)鍵變量為4 個(gè)PSD 傳感器的測(cè)量結(jié)果P1~P4。為得到每個(gè)PSD 的測(cè)量結(jié)果，根據(jù)式（1）設(shè)計(jì)信號(hào)處理電路，如圖4 所示。PSD 傳感器產(chǎn)生的信號(hào)I1和I2是電流信號(hào)，不能直接通過A/DC 進(jìn)行采樣，需要經(jīng)過前置信號(hào)調(diào)理電路，轉(zhuǎn)換成U1和U2后進(jìn)行運(yùn)算和采集。這里U1（U2）和I1（I2）是線性比例關(guān)系，若放大電路倍數(shù)為1，已知2D=70 mm，由式（1）轉(zhuǎn)變?yōu)槭剑?），電壓信號(hào)U1和U2經(jīng)過運(yùn)算得到：

圖4 PSD 信號(hào)處理與采集電路設(shè)計(jì)方案Fig.4 PSD signal processing and acquisition circuit design scheme

其中x為一維光斑位置。

圖4 的前置調(diào)理電路使用積分電路（見圖5），可完成兩個(gè)任務(wù)：一是通過R1和C1將PSD 感應(yīng)電流進(jìn)行I/V轉(zhuǎn)換和信號(hào)放大，以滿足A/DC的信號(hào)接收（±10 V）要求；二是作為硬件濾波方式使用C和Rf進(jìn)行低通濾波，去除絕大部分的高頻電信號(hào)干擾，提高測(cè)量信號(hào)的穩(wěn)定性。

圖5 信號(hào)調(diào)理電路Fig.5 Signal conditioning circuit

2.4 測(cè)量系統(tǒng)軟硬件組成

如圖6 所示，4-PSD 系統(tǒng)由上下位機(jī)組成，下位機(jī)主控為STM32F103C8T6 單片機(jī)，負(fù)責(zé)單個(gè)PSD 傳感器信號(hào)的采集和傳輸。單個(gè)PSD 傳感器的兩路電流信號(hào)通過前置調(diào)理后變成U1和U2電壓信號(hào)，經(jīng)過加法電路、減法電路和模擬除法器AD633 計(jì)算后得到實(shí)際光斑距離信號(hào)，再通過由AD7606 和STM32F103C8T6 單片機(jī)組成的A/DC 采集電路后，與上位機(jī)軟件通過USB-HID協(xié)議傳送數(shù)據(jù)，最高頻率為1 000 Hz。這里采樣頻率為50 Hz，能滿足一般需求。

圖6 4-PSD 測(cè)量系統(tǒng)原理與標(biāo)定裝置Fig.6 Schematic diagram and calibration devices of 4-PSD measuring system

上位機(jī)為PC 上編寫的C#程序界面，主要負(fù)責(zé)USB-HID 通訊、數(shù)據(jù)采集記錄、測(cè)量模型（公式（2））計(jì)算、數(shù)據(jù)可視化和標(biāo)定等功能，如圖7所示。

圖7 4-PSD 測(cè)量系統(tǒng)上位機(jī)程序Fig.7 Man-machine interface of 4-PSD measuring system

3 實(shí)驗(yàn)與結(jié)果分析

3.1 實(shí)驗(yàn)裝置

4-PSD 測(cè)量實(shí)驗(yàn)裝置如圖8 所示，十字激光器通過固定板固定在4-PSD 裝置上空某平面，通過步進(jìn)電機(jī)調(diào)整轉(zhuǎn)角。使用步進(jìn)電機(jī)控制器分別控制精密XY位移平臺(tái)、激光角度調(diào)整機(jī)構(gòu)運(yùn)動(dòng)，以2 個(gè)激光位移傳感器輸出的位置信息作為標(biāo)準(zhǔn)參考值，可對(duì)4-PSD 測(cè)量裝置測(cè)得的3 自由度位置信息進(jìn)行標(biāo)定。

圖8 4-PSD 實(shí)驗(yàn)裝置Fig.8 Photo of 4-PSD experimental setup

3.2 環(huán)境光干擾降低

對(duì)PSD 信號(hào)進(jìn)行預(yù)測(cè)試，結(jié)果如圖9 所示。黑色曲線是對(duì)單個(gè)PSD 其中任意一點(diǎn)只有硬件濾波的信號(hào)調(diào)理電路進(jìn)行實(shí)驗(yàn)觀察的結(jié)果，雖然濾除了高頻干擾，但是仍然存在數(shù)據(jù)波動(dòng)。對(duì)數(shù)據(jù)進(jìn)行快速傅里葉變換分析離散數(shù)據(jù)頻譜，發(fā)現(xiàn)干擾信號(hào)的頻率主要分布在0~5 Hz，屬于低頻干擾，主要為環(huán)境光影響。干擾信號(hào)在一定程度上會(huì)增加系統(tǒng)測(cè)量誤差，因此需要采取降低環(huán)境光干擾的相應(yīng)措施。

圖9 激光調(diào)制去環(huán)境光干擾效果Fig.9 Laser modulation to remove ambient light interference effect

PSD 傳感器在有光的地方會(huì)感應(yīng)出光電流，用單片機(jī)I/O 口輸出脈寬調(diào)制（Pulse Width Modulation， PWM）脈沖驅(qū)動(dòng)十字線激光器，可減弱環(huán)境光干擾的影響。當(dāng)驅(qū)動(dòng)波形為低電平時(shí)，激光器關(guān)，輸出信號(hào)Uoff（僅包含環(huán)境光信號(hào)）；當(dāng)波形為高電平時(shí)，激光器開，輸出信號(hào)Uon（包含環(huán)境光信號(hào)+激光器信號(hào)），一個(gè)周期內(nèi)采樣兩次，經(jīng)單片機(jī)內(nèi)的數(shù)值相減便可得到去環(huán)境光后的采集電壓，如圖9 所示。此信號(hào)為減弱了環(huán)境光干擾幅值后的信號(hào)，但由于激光器開、關(guān)的兩次采樣不是同時(shí)進(jìn)行的，導(dǎo)致信號(hào)采集至少有半個(gè)周期的滯后，所以環(huán)境光產(chǎn)生的低頻干擾只能減弱，不能完全消除，信號(hào)仍有環(huán)境光殘留影響，因此需要通過后續(xù)的軟件濾波和標(biāo)定手段來提高測(cè)量系統(tǒng)的精度。

3.3 實(shí)驗(yàn)誤差分析

標(biāo)定平臺(tái)如圖8 所示，固定十字激光器垂直于4-PSD 裝置的中心原點(diǎn)后，通過步進(jìn)電機(jī)控制的精密XY位移平臺(tái)帶動(dòng)4-PSD 裝置。2 個(gè)激光位移傳感器垂直置于平臺(tái)兩側(cè)，用于記錄4-PSD裝置的平面位移，X軸運(yùn)動(dòng)記為激光位移傳感器X，Y軸為激光位移傳感器Y，同步采集兩者輸出的電壓值與4 個(gè)PSD 傳感器對(duì)應(yīng)輸出的電壓值（6 個(gè)通道）并傳輸至上位機(jī)，圖10 為控制步進(jìn)電機(jī)滑臺(tái)運(yùn)動(dòng)一個(gè)直徑為35 mm 的圓。

圖10 4-PSD 測(cè)量誤差分析Fig.10 Error analysis of 4-PSD measurement

預(yù)實(shí)驗(yàn)發(fā)現(xiàn)，4-PSD 測(cè)量裝置存在激光光源、溫度、環(huán)境光和裝配誤差等影響；另外，PSD信號(hào)在經(jīng)過硬件濾波和激光調(diào)制后，每個(gè)環(huán)節(jié)都引入了信號(hào)失真，雖然信號(hào)質(zhì)量更高，但是卻導(dǎo)致一維PSD 出現(xiàn)線性失真，4-PSD 測(cè)量裝置出現(xiàn)非線性失真。測(cè)量得到的位置信息包含綜合性偏差，最大誤差達(dá)到4.98 mm，如圖10 所示。此外角度旋轉(zhuǎn)變化雖然準(zhǔn)確，但卻會(huì)引起位移的耦合偏差。

3.3.1 數(shù)字濾波算法實(shí)驗(yàn)結(jié)果與分析

父母離婚的那天，父親狠狠地打他，罵他“敗家星”。母親大病了一場(chǎng)，精神恍惚。父親給了微薄的贍養(yǎng)費(fèi)，母親說，如果你要了，就不是我兒子。于是，他只有去打工。父親漸漸成了這個(gè)城市的有錢人，開著名車一次次從他身邊經(jīng)過要把錢給他，他拒絕，他恨這個(gè)男人。母親去世后，4年大學(xué)，他靠助學(xué)貸款，又打了好多工，熬了過來。他不會(huì)原諒父親。父親越來越有錢，還放出話來，窮死他，我一分錢都不會(huì)留給他！他冷笑，我一分錢也不會(huì)要！24歲那年，他得了一場(chǎng)病，醫(yī)生問他家屬是誰，他嚇了一跳，他知道自己病得很重，便打電話給父親。

為了得到最適用于PSD 調(diào)理電路的數(shù)字濾波算法，本文分別進(jìn)行了算術(shù)平均值濾波法、中位值濾波法、中位值平均濾波法、一階滯后濾波法和加權(quán)遞推平均濾波法的實(shí)驗(yàn)，并對(duì)比它們的濾波效果。使光斑照射在PSD 同一位置點(diǎn)，在軟件中分別加入5 種數(shù)字濾波算法，效果如圖11 所示，加入數(shù)字濾波后的數(shù)據(jù)穩(wěn)定性均有不同程度的改善。

圖11 加入多種濾波算法后的信號(hào)Fig.11 Signals after adding variety of filtering algorithms

由圖11 可知，未加入任何濾波算法時(shí)測(cè)試得到的數(shù)據(jù)波動(dòng)較大，經(jīng)過濾波算法后，測(cè)量數(shù)據(jù)的波動(dòng)情況變得平緩。數(shù)據(jù)對(duì)比如表1 所示，雖然略有波動(dòng)，但能滿足系統(tǒng)測(cè)量數(shù)據(jù)的穩(wěn)定性要求。

表1 濾波前后不同濾波算法的數(shù)據(jù)對(duì)比Tab.1 Comparison of various filtering algorithms before and after filtering

在測(cè)試穩(wěn)定性的同時(shí)，不能忽略系統(tǒng)的實(shí)時(shí)性。對(duì)于算術(shù)平均濾波法、中位值濾波法、中位值平均濾波法和加權(quán)遞推平均濾波法，在程序啟動(dòng)后就可以測(cè)出即時(shí)位置數(shù)據(jù)，實(shí)時(shí)性較好；但是一階滯后濾波算法的實(shí)時(shí)性較差，等待較長時(shí)間才能得到測(cè)量值。

如圖11 所示，濾波算法存在失真，所以不同濾波算法得到的測(cè)量數(shù)據(jù)存在波動(dòng)，導(dǎo)致圖上縱坐標(biāo)的絕對(duì)位置不一樣，因此在濾波之后要進(jìn)行位置修正。位置修正后便于觀察誤差，誤差分析如表2 所示。

表2 線性修正后各種濾波算法的數(shù)據(jù)對(duì)比Tab.2 Data comparison of various filtering algorithms after linear correction

綜合分析可知，一階滯后濾波法的實(shí)時(shí)性較差，中位值濾波法和算術(shù)平均濾波法的誤差精度較低。相比之下，中位值平均濾波法的誤差較低，既能過濾隨機(jī)干擾信號(hào)，又能過濾脈沖干擾，有較好的實(shí)時(shí)性、穩(wěn)定性和準(zhǔn)確性。

3.3.2 標(biāo)定方案設(shè)計(jì)

濾波后的4-PSD 測(cè)量系統(tǒng)減弱了隨機(jī)誤差的影響，但誤差的影響因素會(huì)隨使用環(huán)境而變化。實(shí)驗(yàn)發(fā)現(xiàn)，僅濾波處理并不能使測(cè)量裝置適用于各種環(huán)境，由于激光器、調(diào)理電路、采集電路等的綜合影響，其輸出信號(hào)與光斑運(yùn)動(dòng)之間存在一定程度的非線性，需要通過標(biāo)定來補(bǔ)償。

為了進(jìn)一步降低誤差，需要對(duì)裝置進(jìn)行末端的實(shí)測(cè)標(biāo)定。標(biāo)定的參考基準(zhǔn)為激光位移傳感器的測(cè)量結(jié)果，激光位移傳感器的安裝如圖8 所示。采用抗干擾能力較強(qiáng)的Panasonic HGC1100 激光位移傳感器（測(cè)量范圍為±35 mm，分辨率為0.01 mm，精度為0.07 mm），分別進(jìn)行單個(gè)PSD 的線性校正和4-PSD 裝置的二階非線性擬合，進(jìn)一步提高系統(tǒng)穩(wěn)定性和準(zhǔn)確性。

標(biāo)定處理分為逐個(gè)PSD 傳感器的線性校正和4-PSD 合成軌跡的多項(xiàng)式擬合（二階非線性校正）。標(biāo)定的參考為激光位移傳感器輸出，將4-PSD 的輸出結(jié)果向激光位移傳感器的測(cè)量結(jié)果上進(jìn)行逼近。由先前的誤差分析可知，4-PSD 測(cè)量裝置存在線性、非線性誤差。其中線性誤差如圖12 所示，即理論上一維PSD 傳感器的光斑移動(dòng)距離與輸出電壓的關(guān)系是成正比的，因誤差使得測(cè)量數(shù)據(jù)存在線性漂移。

圖12 一維PSD 線性失真Fig.12 One-dimensional PSD linear distortion

控制4-PSD 裝置沿X軸做勻速直線運(yùn)動(dòng)，軌跡數(shù)據(jù)如圖13（a）所示；控制激光器旋轉(zhuǎn)近30°，其位移偏差如圖13（b）所示?？梢钥闯?，標(biāo)定前4-PSD 測(cè)量裝置測(cè)得的數(shù)據(jù)存在明顯的誤差。

圖13 未標(biāo)定前的平面位移和角度旋轉(zhuǎn)變化Fig.13 Changes in plane displacement and angle rotation before calibration

由于4-PSD 測(cè)量裝置是基于各個(gè)PSD 傳感器的信號(hào)輸出，所以首先需要將平臺(tái)上的PSD 傳感器逐個(gè)校正。如圖14 所示，分別移動(dòng)X軸、Y軸，記錄PSD1 的輸出信號(hào)變化。結(jié)合圖13 和圖14 的實(shí)際測(cè)量數(shù)據(jù)可知，4-PSD 測(cè)量裝置存在誤差傾斜角為0.537°，同時(shí)測(cè)量軌跡與實(shí)際軌跡的兩點(diǎn)誤差最大為5.11 mm。因此，實(shí)驗(yàn)中將數(shù)據(jù)一次項(xiàng)擬合，開展補(bǔ)償校正、倍數(shù)校正、原點(diǎn)校正等措施。

圖14 PSD1 輸出與XY 軸直線運(yùn)動(dòng)關(guān)系Fig.14 Relationship of PSD1 output and horizontal XY axis linear motion

移動(dòng)X軸，記錄PSD1對(duì)應(yīng)的位移變化Δx和激光位移傳感器X的輸出xref，代入式（5）進(jìn)行補(bǔ)償校正；移動(dòng)Y軸，記錄PSD1對(duì)應(yīng)的位移變化Δx和激光位移傳感器Y的輸出yref，代入式（6）進(jìn)行倍數(shù)校正。再保持4-PSD 裝置不動(dòng)，根據(jù)式（7）進(jìn)行原點(diǎn)校正，最后代入式（8）即得到PSD1傳感器的線性校正公式。

其中：P1為PSD1傳感器的初始激光感應(yīng)距離，Pmm1為PSD1傳感器經(jīng)過校正后的激光感應(yīng)距離（PSD2，PSD3，PSD4同理）。將4 個(gè)Pmmi代入計(jì)算式（2）和式（3）即可得到坐標(biāo)（x，y，α）。

如圖14 虛線所示，校正后，當(dāng)平臺(tái)沿X軸走直線時(shí)PSD1輸出電壓近似不變，而沿Y軸走直線時(shí)其輸出電壓與相對(duì)位移有線性正比關(guān)系。

控制4-PSD 裝置走圓形軌跡，對(duì)所得到圓形軌跡坐標(biāo)組（x，y）和（xref，yref）進(jìn)行回歸分析（多項(xiàng)式擬合）從而減弱線性、非線性誤差。分別對(duì)上述兩組坐標(biāo)組進(jìn)行一階擬合和二階擬合。最后將得到的單個(gè)PSD 線性校正轉(zhuǎn)換關(guān)系式和4-PSD 坐標(biāo)擬合關(guān)系式結(jié)合，共同作為4-PSD 測(cè)量裝置的標(biāo)定公式。

3.4 標(biāo)定實(shí)驗(yàn)結(jié)果與分析

根據(jù)上述標(biāo)定方案設(shè)計(jì)，標(biāo)定處理分為線性校正和多項(xiàng)式擬合。線性校正主要是去除測(cè)量裝置的線性誤差。多項(xiàng)式擬合中包含了一階擬合與二階擬合，一階擬合是對(duì)線性校正做進(jìn)一步的線性迭代收斂，彌補(bǔ)線性校正過程中的數(shù)據(jù)偏差；二階擬合則是以拋物線方式縮小測(cè)量過程的非線性誤差。

如圖15（a）所示，經(jīng)過線性校正后位移軌跡較為接近真實(shí)值，誤差傾斜角為0.019°，測(cè)量軌跡與實(shí)際軌跡的兩點(diǎn)誤差最大為0.52 mm，有較好的校正效果。而當(dāng)激光器旋轉(zhuǎn)角度近30°時(shí)，4-PSD 測(cè)量裝置測(cè)得的位移誤差如圖15（b）所示，位移誤差會(huì)隨角度的變化而變化，最大誤差為1.38 mm，分析得知該誤差是由于激光器的4 條光線不均勻?qū)е?，為減弱該非線性誤差需要進(jìn)行多項(xiàng)式擬合處理。

圖15 線性校正后的平面位移和角度旋轉(zhuǎn)變化圖Fig.15 Changes in plane displacement and angle rotation after linear correction

在進(jìn)行多項(xiàng)式擬合時(shí)，第一步是控制4-PSD裝置走圓形軌跡，通過記錄到的兩組激光位移傳感器坐標(biāo)數(shù)據(jù)進(jìn)行一階擬合；第二步是4-PSD 裝置再走一次圓形軌跡，進(jìn)行二階擬合。一階擬合后的運(yùn)行軌跡如圖16 所示，最大誤差為0.35 mm。整體上，軌跡接近于真實(shí)位置信息，但在坐標(biāo)（17.5，0）附近的測(cè)量軌跡弧度存在較小的非線性干擾，該區(qū)域的曲線較其他區(qū)域的曲線不夠平滑，且整體誤差較大的部分就在該區(qū)域。

圖16 一階擬合后的圓形軌跡圖Fig.16 Circular trajectory map after first-order fitting

二階擬合完成后，運(yùn)行圓形軌跡如圖17 所示，拋物線的擬合對(duì)非線性誤差有一定的削弱效果，在（17.5，0）附近區(qū)域的軌跡圓滑性得到了較好的改善。此外，二階擬合后圓軌跡的最大誤差為0.26 mm。

圖17 二階擬合后的圓形軌跡Fig.17 Circular trajectory map after second-order fitting

多項(xiàng)式擬合后進(jìn)行角度耦合誤差的測(cè)試，令XY滑臺(tái)不動(dòng)，只控制激光器旋轉(zhuǎn)角度變化，從16.61°旋轉(zhuǎn)至-16.34°。如圖18（a）所示，角度計(jì)算結(jié)果變化平滑且準(zhǔn)確度高，相對(duì)誤差為0.24%。位移耦合偏差如圖18（b）所示，在旋轉(zhuǎn)過程中最大誤差為0.34 mm，因?yàn)樵?7.5 mm 半徑圓形軌跡的誤差為0.26 mm，兩處誤差相差不大，可認(rèn)為屬于同一精度級(jí)別。因此，多項(xiàng)式擬合后角度旋轉(zhuǎn)引起的位移誤差在允許的精度范圍內(nèi)。

圖18 多項(xiàng)式擬合后的角度旋轉(zhuǎn)變化Fig.18 Angle rotation change after polynomial fitting

3.5 標(biāo)定結(jié)果驗(yàn)證

分析實(shí)驗(yàn)結(jié)果曲線可知，測(cè)量系統(tǒng)的工作范圍越大，其誤差影響就越大。為了便于直觀了解測(cè)量系統(tǒng)的實(shí)際誤差分布，如圖19 所示，采用棋盤格均勻布點(diǎn)方式呈現(xiàn)測(cè)量系統(tǒng)經(jīng)標(biāo)定后的測(cè)量精度分布。

圖19 4-PSD 裝置測(cè)量精度分布Fig.19 Accuracy distribution in 4-PSD device measurement

4-PSD 測(cè)量裝置經(jīng)過標(biāo)定后，根據(jù)圖19 采樣點(diǎn)分布，移動(dòng)激光器至布點(diǎn)圖的采樣點(diǎn)上并記錄該點(diǎn)誤差，繪制誤差分布曲面如圖20 所示，繪制誤差分布等值線如圖21 所示。由圖可知，整個(gè)測(cè)量區(qū)域內(nèi)，越接近坐標(biāo)原點(diǎn)，其測(cè)量誤差越小，在位于原點(diǎn)對(duì)稱的20 mm×20 mm 的區(qū)域內(nèi)最大誤差為0.31 mm，邊緣部分由于存在激光器光源質(zhì)量、以及PSD 傳感器感光材料性能等因素，誤差較大，最大時(shí)達(dá)到0.68 mm。整體上，4-PSD測(cè)量裝置標(biāo)定后的精度有明顯改善，全范圍的平均誤差為0.49 mm，比圖10 的誤差降低了90%，可滿足大部分平面3 自由度測(cè)量的應(yīng)用場(chǎng)合要求。

圖20 4-PSD 測(cè)量系統(tǒng)誤差分布曲面Fig.20 Distribution surface of 4-PSD measurement system error

圖21 4-PSD 測(cè)量系統(tǒng)誤差分布等值線Fig.21 Distribution contour of 4-PSD measurement system error

4 結(jié) 論

本文針對(duì)現(xiàn)有機(jī)器人多自由度位姿檢測(cè)方法的不足，提出了一種利用多PSD 傳感器優(yōu)化配置，對(duì)運(yùn)動(dòng)機(jī)構(gòu)平面三自由度進(jìn)行非接觸式檢測(cè)的方法，并搭建了4-PSD 測(cè)量系統(tǒng)，解決了常規(guī)傳感器難以一次性測(cè)量平面3 自由度的問題；得益于PSD 傳感器的高精度和高響應(yīng)頻率，該測(cè)量系統(tǒng)可滿足大部分機(jī)器人末端位姿實(shí)時(shí)測(cè)量反饋的要求。基于PSD 的工作原理設(shè)計(jì)了基于單片機(jī)的一維PSD 傳感器信號(hào)處理電路，得到了電壓信號(hào)與光斑位置的映射關(guān)系；其次，為了使4-PSD 能在具有環(huán)境光干擾的場(chǎng)景下使用，對(duì)數(shù)據(jù)采集時(shí)的信號(hào)干擾問題開展硬件濾波、激光調(diào)制和數(shù)字濾波等實(shí)驗(yàn)，對(duì)PSD 從信號(hào)端到輸出端進(jìn)行處理，處理后的信號(hào)干擾變少，消除了環(huán)境光影響；但信號(hào)處理同時(shí)引入信號(hào)的線性和非線性失真，最后，通過分析4-PSD 測(cè)量裝置的線性、非線性誤差，設(shè)計(jì)了包括線性校正、多項(xiàng)式擬合的標(biāo)定方法，彌補(bǔ)了環(huán)境光信號(hào)處理引入的失真，標(biāo)定后系統(tǒng)在20 mm×20 mm 區(qū)域內(nèi)的最大誤差僅為0.31 mm。實(shí)驗(yàn)結(jié)果表明，在70 mm×70 mm 內(nèi)測(cè)量系統(tǒng)的平均誤差為0.49 mm，比標(biāo)定前降低了90%。本文的測(cè)量裝置和方法為機(jī)器人末端高精度、高速率的實(shí)時(shí)測(cè)量反饋提供了技術(shù)手段和理論支撐。