郭云鵬，張 弓,侯至丞,王衛(wèi)軍,2，王 力，韓彰秀,2

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

0 引言

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

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

1 主從映射模型

根據(jù)主手與從手的相對結(jié)構(gòu)，主從操作系統(tǒng)可以分為主從同構(gòu)型和主從異構(gòu)型兩種結(jié)構(gòu)。主從手結(jié)構(gòu)如圖1所示。

圖1 主從手結(jié)構(gòu)示意圖

主從手構(gòu)型的優(yōu)點(diǎn)是主從映射較為簡單，從手末端位姿控制易于實(shí)現(xiàn);缺點(diǎn)是受從手結(jié)構(gòu)的限制，通用性差，難以達(dá)到人機(jī)工程學(xué)要求。主從異構(gòu)型主手采用完全獨(dú)立的設(shè)計(jì)，具有較好的通用性，且能兼具人機(jī)工程學(xué)的設(shè)計(jì)要求；缺點(diǎn)是控制模型更為復(fù)雜，需要實(shí)時求解主從系統(tǒng)的正反運(yùn)動學(xué)甚至動力學(xué)[8]。本文主手為市場通用的三維鼠標(biāo)SpaceMouse，從手為經(jīng)典Stewart平臺，屬于異構(gòu)型主從結(jié)構(gòu)。但由于主手的通用性，其在人機(jī)交互上具有易用、可靠的特點(diǎn)。

1.1 主從映射方式

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

表1 主從手自由度映射

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

圖2 主從運(yùn)動映射流程圖

1.2 運(yùn)動學(xué)反解

從圖2可知，為實(shí)現(xiàn)從手Stewart跟隨主手Space Mouse運(yùn)動，除了建立主從手間的運(yùn)動映射關(guān)系外，還需計(jì)算Stewart平臺的運(yùn)動學(xué)反解。

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

假設(shè)動平臺的位姿廣義坐標(biāo)為q=[x,y,z,α,β,γ]。其中：x、y、z為動坐標(biāo)系原點(diǎn)p在靜坐標(biāo)系中的坐標(biāo)；α、β、γ為動坐標(biāo)系相對于靜坐標(biāo)系的∠RPY表示。根據(jù)∠RPY及旋轉(zhuǎn)變換理論，可得相應(yīng)的旋轉(zhuǎn)矩陣：

圖3 Stewart平臺坐標(biāo)系

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

(1)

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

(2)

2 運(yùn)動平滑性分析

通過人工操控主手時，會不可避免地產(chǎn)生一定的抖動。由圖2可知，主手端的抖動必然會復(fù)現(xiàn)至從手端，從而影響從手的響應(yīng)，過高頻率的抖動甚至?xí)p壞驅(qū)動電機(jī)。因此，有必要對主手端的輸出進(jìn)行一定的平滑處理[11]。

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

2.1 原理分析

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

(3)

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

(4)

根據(jù)Savitsky和Golay的發(fā)現(xiàn)，利用卷積運(yùn)算可以求取擬合多項(xiàng)式的常數(shù)項(xiàng)，即對輸入數(shù)據(jù)進(jìn)行加權(quán)平均：

(5)

由微積分知識可知，若要ε最小，ε對各個參數(shù)的偏導(dǎo)數(shù)都應(yīng)為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)

經(jīng)計(jì)算可得：

(10)

于是有：

Ba=ATAa=ATx

(11)

a=(ATA)-1ATx=Hx

(12)

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

2.2 仿真分析

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

3 試驗(yàn)研究

為實(shí)現(xiàn)主手對從手的遙操作，本文借助以太網(wǎng)組建控制系統(tǒng)，包括Ethernet和Ethercat通信。Ethernet負(fù)責(zé)主手與控制器，即上位機(jī)與下位機(jī)之間的數(shù)據(jù)傳輸；Ethercat負(fù)責(zé)控制器與伺服驅(qū)動之間的數(shù)據(jù)交換?？刂破鞑捎玫聡鳥eckhoff公司的CX5120，伺服驅(qū)動為埃斯頓ProNet系列交流伺服。系統(tǒng)結(jié)構(gòu)如圖4所示。

圖4 系統(tǒng)結(jié)構(gòu)圖

本文組建的試驗(yàn)樣機(jī)以Beckhoff公司的TwinCat作為下位機(jī)程序的開發(fā)環(huán)境，以Visual Studio MFC結(jié)合3DConnexion公司提供的SDK作為上位機(jī)開發(fā)環(huán)境。所設(shè)計(jì)上位機(jī)測試界面分兩部分：一部分為SpaceMouse數(shù)據(jù)顯示，包括直接采集數(shù)據(jù)以及Stewart計(jì)算缸長；另一部分為Stewart數(shù)據(jù)顯示，包括實(shí)際位姿和實(shí)際缸長。

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

表2 Stewart平臺最大運(yùn)動范圍

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

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

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

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

4 結(jié)束語

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