六自由度并聯微動機器人全閉環控制系統研究

豐 茂

（國家知識產權局專利局專利審查協作北京中心，北京100190）

微動并聯機器人因具有運動精度和分辨率高、自身體積小、剛度大等特點，目前已在MEMS制造、光學調整、激光制導、生物醫療、IC制造等微操作領域得到了廣泛的應用［1－4]．但是由于制造及裝配等誤差因素，使得并聯微動機器人的精確定位不易實現．針對提高定位精度的方法，已進行了很多相關的研究．吳江寧等［5]通過經緯儀測量運動平臺的系列變化位姿，間接計算六自由度平臺的實際結構參數并在控制軟件中進行補償以提高位姿精度．陸敏智等［6]研究了6－THHT型并聯機器人基于多傳感器數據融合技術的反饋測控系統，探討了并聯機器人六路驅動桿的閉環反饋控制的實現方法，為直接對動平臺實現全閉環控制提供了理論依據．Rainer Tutsch等［7]利用集成于并聯機器人的3D傳感器，設計配置了基于多通路線陣CCD圖像采集設備的測量系統，使得機器人的定位精度達到1μm．

為了提高精度，本文提出一種基于集成傳感器的新型三桿六自由度并聯微動機器人的末端位姿檢測及全閉環控制的方法．

1 結構組成

圖1為3－PPSR并聯微動機器人的結構示意圖．三根固定長度的連桿3連接動平臺1和底座9，支桿上端采用單軸柔性鉸鏈2作為輸出，下端采用柔性球鉸鏈5作為輸入，支桿上端三個鉸鏈中心點和下端三個鉸鏈中心點均呈等邊三角形分布．下平臺包含整體加工的三個二維微動平臺4，采用了結構簡潔的直角雙層彈性平板結構．測量末端位姿所需的測量輔助裝置6與動平臺固連，7與底座固連．測量所用的傳感器8安裝在7上．

并聯機構的上下平臺坐標中心到柔性鉸鏈中心點的距離分別為10mm、50mm，桿長為60mm．

圖1 3－PPSR并聯微動機器人結構示意圖

每條支鏈均采用雙輸入結構，即由2個壓電陶瓷驅動雙輸入彈性平板來實現X和Y兩方向的輸入，驅動采用PZ公司的PSt150／7／7vs12型壓電陶瓷．3條支鏈輸入端總計3組雙彈性平板、6個壓電陶瓷均集成在基座上，由于每條支鏈輸入為2個自由度，因而在3個支鏈末端可實現六自由度微運動．

設下端3個球鉸中心點分別為Bi（i＝1，2，3），建立如圖2所示坐標系B－XYZ，原點為B（三角形中心）．同理，設上端3個單軸柔性鉸鏈中心點分別為Pi（i＝1，2，3），動坐標系原點P建在Pi中心，PX＇Y＇Z＇的X＇軸與B－XYZ的X軸平行，P－X＇Y＇Z＇的Z＇軸與B－XYZ的Z軸重合．

設b為B到Bi長度，同理，p為P到Pi長度．零位時Bi點坐標為［b·cosλi，b·sinλi，0] ，零位時Pi點在動坐標系中坐標為［p·cosλi，p·sinλi，0] ．其中i＝1，2，3；λi＝（i－1）×120°．△Mi和△Ni分別為壓電陶瓷的輸出位移，其中i＝1，2，3，且i＝1時，i＋1，i＋2分別代表2，3；i＝2時，i＋1，i＋2分別代表3，1；i＝3時，i＋1，i＋2分別代表1，2．動坐標系沿固定坐標系X軸微小平移△x，沿Y軸微小平移△y，沿Z軸微小平移△z，繞X軸旋轉微小角位移△α，繞Y軸旋轉微小角位移△β，繞Z軸旋轉微小角位移△γ，通過幾何關系對微動并聯機器人進行逆

圖2 并聯微動機器人參考坐標系

解可得

寫成矩陣的形式，可以得到微動并聯機器人的位置逆解方程：

式中：J為常數矩陣，所以其正解方程為

預緊力對壓電陶瓷的最終輸出有著較大的影響，在空載時壓電陶瓷在施加最大電壓（150V）時最大輸出為9μm，預緊后僅為7μm，所以在進行工作空間仿真時，取壓電陶瓷的實際最大行程為7μm．圖3所示為運用Matlab數學計算軟件對微動機器人的工作空間進行仿真的效果圖．

圖3 微動機器人工作空間仿真效果圖

通過對工作空間的仿真，可以得到3－PPSR微納米操作機器人在單個X或Y向上的行程大約為±4μm，在Z向上的行程為5μm．

2 測量方法

為了實現并聯微動機構的末端位姿全閉環控制，增加了傳感器測量裝置，并將其與并聯機器人集成，構建由壓電陶瓷器件、壓電陶瓷驅動電源、微位移傳感器、微定位控制器和微動機構組成的位置全閉環控制系統，以實現機構、驅動、末端檢測反饋一體化．

為了能夠同時測量6個自由度，那么需要6路測量裝置，本文測量所需的傳感器采用MTI公司的ASP－1－ILR探頭式電容傳感器．將6個電容傳感器以圖4所示的方式分布在相互垂直的3個平面上，并用緊釘螺釘固定在探頭連接裝置上．為了便于計算，在安裝傳感器時，應使傳感器所在坐標系與微動并聯機器人的動平臺所在坐標系平行．由于被測量裝置與動平臺是緊固連接的，所以傳感器測被測裝置實際上就是用傳感器直接測量上平臺．下面以一個面上的兩個傳感器為例，介紹測量的步驟和方法．

圖4 集成了傳感器的測量裝置示意圖

如圖5所示，S1，S2分別為初始狀態時兩個傳感器所對應的投影在被測裝置的位置，為了便于計算，將S1，S2對稱分布于坐標系原點兩側，兩者之間的距離為固定值D，且平行于y軸，測量方向沿x軸向．S′1，S′2為動平臺運動后在被測裝置上傳感器投影的位置，那么ΔXS1，ΔXS2分別為被測裝置即動平臺在固定的傳感器探頭所在直線方向上的位移，該值可由傳感器直接讀出．

圖5 測量幾何關系圖

3 閉環控制及誤差補償

3．1 誤差補償模型

通過傳感器測得的位姿與期望位姿的比較，建立如圖6所示的微動并聯機器人的位姿誤差補償模型．

圖6 微動并聯機器人位姿誤差補償原理圖

設X（p）為機器人的期望位姿，通過逆變換，可得出機器人末端到達X（p）時的壓電陶瓷的輸出位移M，所對應的驅動電壓為U．但由于機器人機構存在各種誤差Δp，這樣當各壓電陶瓷輸出位移為M時，末端傳感器測得的機器人末端的實際位姿為X（p＋Δp），通過逆變換，可以得到末端的實際位姿為X（p＋Δp）時的壓電陶瓷的輸出位移為MΔM，所以得到誤差e＝ΔM．

為了校正機器人末端的位姿，用一個預置的位姿偏移量－Δp，使機器人末端的指令位姿預置為X（p－Δp），即對各輸入電壓進行修正，使得壓電陶瓷驅動電壓為U＋ΔU，補償各壓電陶瓷一定的輸出位移ΔM，此時對應的壓電陶瓷的輸出位移為M＋ΔM，預置的位姿偏移量－Δp與機器人機構誤差Δp相互抵消，即得到機器人的期望位姿X（p）．在這個過程中，需要循環多次的進行誤差補償，以達到所需要的定位精度．

3．2 控制方法研究

傳統的PID控制器是利用一組固定的參數進行控制，不能兼顧動態和靜態、設定值和抑制擾動之間的矛盾．因此，我們引入模糊推理，在初值基礎上適當調整PID的參數，從而改善系統的控制效果．利用模糊控制規則在線對PID參數進行修改，便構成了模糊PID控制器［8]，模糊PID控制器是以偏差e和偏差變化率ec作為輸入，利用模糊規則對比例、積分和微分參數進行在線調節．e（nT）和ec（nT）定義如下：

式中：T為為采樣時間；r（nT）為給定的理論值；y（nT）為第n個采樣時刻實際值．

在進行閉環PID控制時，積分參數KI會增加系統響應的快速性，消除靜差，但其值過大會引起系統的超調，甚至不穩定；比例參數KP過大容易引起系統振蕩，反而使調節時間加長，且當KP太大時，系統將趨于不穩定狀態；微分參數KD對系統性能的影響很小，因此一般KD的取值可以忽略．為簡化控制設計，將數字PID控制器簡化為數字PI控制器，忽略微分項的作用，其結構如圖7所示．

圖7 模糊PI控制原理圖

模糊控制器以誤差e和誤差變化ec作為輸入，PID參數模糊自整定是找出PID中3個參數與e和ec之間的模糊關系，在運行中通過不斷檢測e和ec，根據模糊控制原理來對3個參數進行在線修改，以滿足不同e和ec時對控制參數的不同要求，而使被控對象有良好的動、靜穩態性能．

3．3 控制系統的設計

在對3－PPSR微動并聯機器人的各結構參數進行辨識的基礎上，用辨識結果修正系統運動學模型來實現機器人末端位姿的補償，其具體過程如圖8所示，主要分為以下幾個步驟：

（1）取一期望的末端位姿rd，根據理想的運動學逆解方程計算此時的壓電陶瓷位移．

（2）將壓電陶瓷定位到上述位置，測量出此時的末端位姿ra，并與期望位姿比較，得出末端位姿誤差δr．

（3）如果末端位姿誤差滿足要求，則結束；否則進入下一步驟．

（4）依據微動并聯機器人運動學逆解公式的特點，可以得到產生末端位姿誤差所需的壓電陶瓷的輸出位移．

（5）通過模糊PID算法，得到對應的壓電陶瓷所需位移．

（6）重復步驟（2）～（5），直至動平臺實際末端位姿能夠足夠地接近理想值，從而達到閉環控制的目的．

圖8 微動并聯機器人閉環控制流程圖

4 閉環控制實驗

表1為微動機器人在開環、標定和全閉環控制后的定位精度對比，表2為微動機器人在開環和全閉環控制后的重復定位精度對比．

表1 定位精度對比

表2 重復定位精度對比

由表1和2可知，全閉環控制后，3－PPSR微動并聯機器人的定位精度和重復定位精度均得到了很大提高，并且基本達到期望的末端位姿，從而實現了微動并聯機器人末端位姿準確定位．

5 結束語

本文研制出了機構、驅動、位姿檢測和閉環控制一體化的3－PPSR高精度微動并聯機器人．主要研究工作和結論如下：

（1）采用電容式微位移傳感器，提出針對3－PPSR微動并聯機器人的非接觸式六點測量方法對位姿進行檢測．分析了該方法的安裝和測量誤差，通過對傳感器安裝位置的比較，獲得了最佳的傳感器安裝方案．

（2）利用高精度電容測微儀得到的末端位姿作為反饋信號，采用模糊PID控制算法，實現了對3－PPSR微動并聯機器人的全閉環控制．實驗結果表明，執行全閉環控制后，微動機器人的定位精度和重復定位精度均滿足性能指標要求．

［1] 矯杰．3－PPSR微動并聯機器人的研究［D] ．哈爾濱：哈爾濱工業大學，2006．

［2] 賈慶軒，魏秋霜，孫漢旭，等．并聯微動機器人的研究現狀［J] ．山東理工大學學報：自然科學版，2003，17（6）：98－102．

［3] Kallio P，Lind M，Zhou Q，et al．A 3DOF piezohydraulic parallel micromanipulator［C] ／／Katholieke U L．International Conference on Robotics and Automation．Leuven：IEEE Robotics and Automation Society，1998：1 823－1 828．

［4] Zhang W J，Zou J，Watson L G，et al．The constant－jacobian method for kinematics of a three－DOF planar micro－motion stage［J] ．Journal of Robotic Systems，2002，19（2）：63－72．

［5] 吳江寧，左愛秋，李世倫．并聯六自由度平臺機構機械誤差分析與檢測［J] ．中國機械工程，1999，10（6）：614－617．

［6] 陸敏智．6－THHT并聯機器人的多傳感器測控系統研究［D] ．南京：南京理工大學，2006．

［7] Rainer T，Marcus P，Michael B，et al．3D－sensor for the control of a micro assembly robot［C] ／／George K K．Optomechatronics Systems IV．San Diego：Society of Photographic Instrumentation Engineers，2003：266－273．

［8] 儲岳中，陶永華．基于MATLAB的自適應模糊PID控制系統計算機仿真［J] ．安徽工業大學學報，2004，21（1）：49－52．