采用SG平滑濾波的Stewart平臺主從控制研究

郭云鵬，張 弓,侯至丞,王衛軍,2，王 力，韓彰秀,2

(1.廣州中國科學院先進技術研究所，廣東 廣州 511458；2.深圳市中科德睿智能科技有限公司，廣東 深圳 518055)

0 引言

機器人的離線路徑規劃一般適用于已知作業環境。對于未知環境的完全自主在線路徑規劃，需要引入各類傳感器及復雜的控制算法，成本高、難度大。主從式機器人控制方式是在機器人控制中引入主手設備，以機器人作為從手，通過人操控主手間接控制機器人，是未知環境中機器人的主要路徑規劃方式[1]。美國的達芬奇手術系統(Da Vinci surgical system,DVSS) 外科手術機器人系統[2]、英國的Endo Assist機器人輔助內窺鏡操作系統[3]、天津大學主導研制的“妙手”微創外科手術機器人系統[4]，以及美國NASA的空間站遙操作人形機器人[5]，均采用主從控制，實現手術機器人的操控。當前，主從控制研究的關鍵在于從手跟隨主手運動的平滑度與實時性。

Stewart并聯平臺具有6個自由度，可以完成復雜的任務指令，廣泛應用于運動模擬、裝配、檢測等領域[6]。SpaceMouse三維鼠標通過內置光學傳感器實現6自由度的運動數據采集，便于控制工程模型和虛擬攝像機的空間位置[7]。將SpaceMouse用作Stewart平臺主從控制的主手，可以在自由度上形成一一對應，從而增加操控的自然性和舒適性。為此，本文以Stewart并聯機器人作為控制對象，以三維鼠標SpaceMouse作為控制主手，實現了兩者之間的主從控制，并對其平滑度與實時性進行了試驗研究。

1 主從映射模型

根據主手與從手的相對結構，主從操作系統可以分為主從同構型和主從異構型兩種結構。主從手結構如圖1所示。

圖1 主從手結構示意圖

主從手構型的優點是主從映射較為簡單，從手末端位姿控制易于實現;缺點是受從手結構的限制，通用性差，難以達到人機工程學要求。主從異構型主手采用完全獨立的設計，具有較好的通用性，且能兼具人機工程學的設計要求；缺點是控制模型更為復雜，需要實時求解主從系統的正反運動學甚至動力學[8]。本文主手為市場通用的三維鼠標SpaceMouse，從手為經典Stewart平臺，屬于異構型主從結構。但由于主手的通用性，其在人機交互上具有易用、可靠的特點。

1.1 主從映射方式

SpaceMouse可以實現6個自由度的運動輸出，包括前后、左右方向的側移和側傾，上下的拉伸以及周向的扭轉。Stewart平臺具有前后、左右的平移，升降，俯仰，偏航以及橫滾6個自由度。因此，在自由度上，主從手之間具有一一對應的關系。為符合人的自然操作習慣，對主從手間作自由度上的映射，如表1所示。

表1 主從手自由度映射

基于主從手間在運動自由度上的一一對應關系，主從運動映射既可以是空間點對點映射，也可以是增量式映射。增量式映射輸入的是相對位姿，控制更為簡單且誤差可控，尤其適用于主手工作空間較小且靈敏度高的情況[9]。

圖2 主從運動映射流程圖

1.2 運動學反解

從圖2可知，為實現從手Stewart跟隨主手Space Mouse運動，除了建立主從手間的運動映射關系外，還需計算Stewart平臺的運動學反解。

根據并聯機構相關理論，建立Stewart平臺坐標系[10]如圖3所示。圖3中，Ai(i=1,2,…,6)為上平臺鉸點中心，Bi(i=1,2,…,6)為下平臺鉸點中心。靜坐標系O-XYZ與下平臺固聯，原點O位于下平臺中心，OX軸和OY軸都在靜平臺平面內并且OY垂直于B3B4；動坐標系p-xyz與上平臺固聯，原點p位于上平臺中心，px軸和py軸都在動平臺平面內并且py垂直于A3A4。

假設動平臺的位姿廣義坐標為q=[x,y,z,α,β,γ]。其中：x、y、z為動坐標系原點p在靜坐標系中的坐標；α、β、γ為動坐標系相對于靜坐標系的∠RPY表示。根據∠RPY及旋轉變換理論，可得相應的旋轉矩陣：

圖3 Stewart平臺坐標系

R=Rot(z,γ)Rot(y,β)Rot(x,α)=

(1)

由式(1)可得第i根驅動桿的長度為：

(2)

2 運動平滑性分析

通過人工操控主手時，會不可避免地產生一定的抖動。由圖2可知，主手端的抖動必然會復現至從手端，從而影響從手的響應，過高頻率的抖動甚至會損壞驅動電機。因此，有必要對主手端的輸出進行一定的平滑處理[11]。

本文采用Savitzky-Golay(SG)濾波方法，對運動數據進行平滑處理。SG濾波是一種在時域內基于局域多項式最小二乘法擬合的濾波方法。其最大特點是在濾除噪聲的同時可以確保信號的形狀、寬度不變，因而被廣泛運用于圖像處理中[12]。

2.1 原理分析

考慮一組以n=0為中心的(2M+1)個數據x[n]，利用多項式對其擬合：

(3)

得到最小二乘擬合殘差為：

(4)

根據Savitsky和Golay的發現，利用卷積運算可以求取擬合多項式的常數項，即對輸入數據進行加權平均：

(5)

由微積分知識可知，若要ε最小，ε對各個參數的偏導數都應為0，即：

(6)

化簡得到：

(7)

式中：i=0,1,…,N。

引入一個(2M+1)行、(N+1)列的輔助矩陣A，令：

A={an,i}

(8)

式中：an,i=ni,-M≤n≤M,0≤i≤N。

引入另一個輔助矩陣B，使得：

B=ATgA

(9)

經計算可得：

(10)

于是有：

Ba=ATAa=ATx

(11)

a=(ATA)-1ATx=Hx

(12)

式中：x=[x[-M] …x[M]]T；a=[a[0] …x[N]]T；H為所求卷積系數。

2.2 仿真分析

為驗證SG濾波平滑效果，在Matlab中編寫程序測試。給定一個長度為1 000的正弦波信號，在該信號的基礎上加入服從正態分布的隨機噪聲，并分別選取多項式擬合的數據長度以及階數：①長度30，階數3；②長度30，階數1；③長度10，階數3。經過SG平滑處理前后的濾波效果對比可知，SG濾波在平滑數據的基礎上,很好地保留了原來信號的形狀和寬度。通過對比不同多項式擬合的數據長度和階數的平滑效果(即數據個數2M+1和階次k的不同組合)可以發現：階數越大，越接近原信號形狀，平滑效果越差；數據長度越大，平滑效果越明顯，對原信號形狀的保持越低。該結果符合多項式擬合的一般規律。

3 試驗研究

為實現主手對從手的遙操作，本文借助以太網組建控制系統，包括Ethernet和Ethercat通信。Ethernet負責主手與控制器，即上位機與下位機之間的數據傳輸；Ethercat負責控制器與伺服驅動之間的數據交換。控制器采用德國Beckhoff公司的CX5120，伺服驅動為埃斯頓ProNet系列交流伺服。系統結構如圖4所示。

圖4 系統結構圖

本文組建的試驗樣機以Beckhoff公司的TwinCat作為下位機程序的開發環境，以Visual Studio MFC結合3DConnexion公司提供的SDK作為上位機開發環境。所設計上位機測試界面分兩部分：一部分為SpaceMouse數據顯示，包括直接采集數據以及Stewart計算缸長；另一部分為Stewart數據顯示，包括實際位姿和實際缸長。

由于本文采用的主從映射為增量式映射，如果不對主手的運動輸出量加以限制，必然會造成從手運動超出工作空間。因此，在試驗之前必須先確定Stewart平臺的工作空間。Stewart平臺工作空間由平臺結構、電動缸行程、虎克鉸轉角范圍等決定。以這些決定因素為判定條件，通過空間遍歷的方法，結合前文提到的運動學反解，即可完成求解。本文求解Stewart平臺在6個自由度上的最大運動范圍如表2所示。

表2 Stewart平臺最大運動范圍

主從控制下的驅動缸長度變化如圖5所示。

圖5 主從控制下的驅動缸長度變化曲線

試驗第一階段，在不加入平滑濾波的情況下遠程操控主手，獲取從手Stewart平臺的計算缸長與實際缸長。選取1號缸為例，從圖5(a)可以看出，從手的延時大約為10 ms。但由于沒有對主手進行運動平滑，導致從手的跟隨運動有很明顯的頓挫感。

試驗第二階段，在上位機程序中加入SG平滑濾波。其中，濾波系數設置數據長度為100，階數為3。操控主手獲取Stewart平臺計算缸長與實際缸長。仍然以1號缸為例，如圖5(b)所示。從手的跟隨運動相較于未加濾波時更為平滑，但相應的延時增加到接近40 ms。由此可見，SG具有顯著的平滑效果，但同時也增加了滯后。

4 結束語

針對機器人在特殊環境下的特殊作業要求，本文以經典6自由度并聯機器人Stewart平臺作為研究對象，建立以通用三維鼠標為主手的主從控制系統。分析了主從間的映射模型以及運動平滑，搭建了試驗平臺。試驗表明，本文建立的主從控制，實現了從手跟隨主手的平滑運動。由于使用了數字濾波器，從手具有40 ms左右的延時，符合一般機器人作業的實時性要求。