主從遙操作機(jī)械手工作空間覆蓋控制策略研究

李強(qiáng)， 曾宇飛， 武圣杰， 張華良

（1.沈陽工業(yè)大學(xué)機(jī)械工程學(xué)院， 遼寧沈陽 110870；2.中國科學(xué)院沈陽自動化研究所， 遼寧沈陽 110017）

0 前言

隨著科技的發(fā)展， 人們的社會生產(chǎn)活動越來越智能化， 機(jī)器人技術(shù)發(fā)展迅速［1］。 然而由于相關(guān)技術(shù)的限制， 全自主式的智能機(jī)器人難以實(shí)現(xiàn)， 遙操作機(jī)器人成為一種可行的方法［2］。

在過去幾十年中， 遙操作機(jī)器人發(fā)展迅速且在很多領(lǐng)域得以應(yīng)用， 如醫(yī)療手術(shù)、 航空航天、 水下作業(yè)、 惡劣環(huán)境工作等［3］。 遙操作機(jī)器人能夠?qū)崿F(xiàn)人的主觀能動性， 再輔以智能化功能， 能夠高效地完成任務(wù)［4－5］。 主從式遙操作機(jī)器人是一種典型的遙操作機(jī)器人， 結(jié)合臨場感技術(shù)和虛擬現(xiàn)實(shí)技術(shù)后， 大大提高了作業(yè)能力［6］。 在主從式遙操作機(jī)器人中， 操作者操控主機(jī)器人控制從機(jī)器人進(jìn)行現(xiàn)場作業(yè)。 然而由于主手的尺寸較小且結(jié)構(gòu)與從手有差異， 主手的工作空間過小， 通過比例系數(shù)放大主手的工作空間又導(dǎo)致與從手不匹配［7－8］。 為此， 本文作者設(shè)計(jì)一種區(qū)域覆蓋算法， 通過將主手工作空間區(qū)域覆蓋到從手上， 以解決主手工作空間過小和不匹配等問題。

1 位姿控制方法

文中設(shè)計(jì)的控制方法分為3 個(gè)部分： 區(qū)域覆蓋位置控制、 相對姿態(tài)控制、 比例映射控制。

在遙操控機(jī)器人中， 主手和從手是一一對應(yīng)的關(guān)系， 在循環(huán)掃描過程中， 主手將位姿信息傳遞給從手， 從手追蹤移動到該位姿［9］。 文中的位置控制方法將主從手一一對應(yīng)關(guān)系轉(zhuǎn)換為多次映射關(guān)系， 以達(dá)到主手工作空間覆蓋目的。 同時(shí)， 為了更安全和便利地操作主從手進(jìn)行作業(yè)， 設(shè)計(jì)了相對姿態(tài)方法和比例映射方法。

1.1 主手運(yùn)動學(xué)建模

文中設(shè)計(jì)的控制方法主要是完成位置和姿態(tài)的控制， 使用Touch 手柄達(dá)成目標(biāo)。 手柄結(jié)構(gòu)簡圖如圖1所示。

使用改進(jìn)D－H 參數(shù)法建立主操作手坐標(biāo)系［10］，如圖1 所示， 其中｛O0X0Y0Z0｝ 為主手的世界坐標(biāo)系（主手基坐標(biāo)系）。 主手后3 個(gè)關(guān)節(jié)為姿態(tài)坐標(biāo)系， 滿足三軸交于一點(diǎn)［11］， 末端點(diǎn)位于｛O5X5Y5Z5｝坐標(biāo)系原點(diǎn)。

建立運(yùn)動學(xué)方程為

連桿長度、 連桿扭角、 連桿距離等參數(shù)已知， 通過獲取各個(gè)連桿夾角即可算出運(yùn)動學(xué)方程。

1.2 區(qū)域覆蓋位置控制算法

位置控制算法是通過主從手建立多次不同的連接， 使主手的工作空間多次映射到從手上， 完成工作空間的區(qū)域覆蓋， 如圖2 所示。

圖2 算法簡圖Fig.2 Schematic of algorithm

算法實(shí)現(xiàn)原理如圖3 所示。 圖中｛O0X0Y0Z0｝坐標(biāo)系為從手的世界坐標(biāo)系， ｛O1X1Y1Z1｝和｛O2X2Y2Z2｝為多次映射建立的主手世界坐標(biāo)系， ｛P1XP1YP1ZP1｝和｛P2XP2YP2ZP2｝是對應(yīng)主手世界坐標(biāo)系下建立的工作坐標(biāo)系。

圖3 位置算法原理Fig.3 Principle of position algorithm

初次建立連接時(shí)， 主從手世界坐標(biāo)系重合， 形成一一對應(yīng)的映射［12］。 移動主手控制從手在世界坐標(biāo)系｛O0X0Y0Z0｝ 下運(yùn)動。 斷開連接后， 記錄從手最終位置P1， 建立工作坐標(biāo)系｛P1XP1YP1ZP1｝。 再次建立主從手連接后， 以點(diǎn)P1為主手末端， 建立主手世界坐標(biāo)系｛O1X1Y1Z1｝， 主手位置即為點(diǎn)P1在坐標(biāo)系｛O1X1Y1Z1｝的坐標(biāo)， 移動主手控制從手末端在坐標(biāo)系｛O1X1Y1Z1｝下運(yùn)動， 主手工作空間即為坐標(biāo)系｛O1X1Y1Z1｝下對應(yīng)區(qū)域。 求得坐標(biāo)系｛O1X1Y1Z1｝相對從手世界坐標(biāo)系｛O0X0Y0Z0｝的位姿矩陣， 即可把點(diǎn)P1在坐標(biāo)系｛O1X1Y1Z1｝的坐標(biāo)轉(zhuǎn)換為在坐標(biāo)系｛O0X0Y0Z0｝ 下的坐標(biāo)， 由此控制從手在世界坐標(biāo)系｛O0X0Y0Z0｝下運(yùn)動。

再次斷開連接后， 記錄從手最后位置P2。 同理建立第三次主從手連接， 以點(diǎn)P2為主手末端點(diǎn)建立主手世界坐標(biāo)系｛O2X2Y2Z2｝， 將P2在｛O2X2Y2Z2｝坐標(biāo)系下的坐標(biāo)轉(zhuǎn)化為從手世界坐標(biāo)系｛O0X0Y0Z0｝下的坐標(biāo)， 即可控制從手運(yùn)動。 通過重復(fù)變換， 可以實(shí)現(xiàn)位置的覆蓋算法。

所以姿態(tài)矩陣可以分解為姿態(tài)矩陣和位置矩陣：

通過不斷的連接－斷開， 可到達(dá)位置覆蓋從手工作空間。

1.3 相對姿態(tài)控制算法

機(jī)器人的姿態(tài)是指機(jī)器人在空間中的方位。 在串聯(lián)型主手中， 由于各關(guān)節(jié)是串聯(lián)連接， 位姿不分離，主手位置改變時(shí)主手的姿態(tài)也會改變， 在操作過程中容易引起誤差［15－16］， 因此文中設(shè)計(jì)的控制方法中，從手的位置和姿態(tài)分別由單獨(dú)的開關(guān)控制。 當(dāng)斷開主從手姿態(tài)連接時(shí)， 主手停止傳遞姿態(tài)信息給從手， 以此規(guī)避位姿不分離所造成的影響。

姿態(tài)控制分為2 個(gè)模式： 絕對模式和相對模式。2 個(gè)模式通過單獨(dú)的開關(guān)來控制連接與中斷。 在離目標(biāo)件較遠(yuǎn)的安全位置時(shí)， 可使用絕對模式， 復(fù)位主從手姿態(tài)角度； 在離目標(biāo)件較近的位置時(shí)， 使用相對模式， 能更加便利地進(jìn)行作業(yè)。

當(dāng)絕對模式連接時(shí)， 主手的世界坐標(biāo)系會與從手的世界坐標(biāo)系重合， 主從手是一一對應(yīng)的映射關(guān)系［17］， 如圖4 所示。

主手世界坐標(biāo)系｛O1X1Y1Z1｝ 與從手世界坐標(biāo)系｛O0X0Y0Z0｝ 重合， 主手通過獲取末端點(diǎn)P1姿態(tài)信息傳遞給從手， 控制從手的姿態(tài)方位［18］。

通過位姿矩陣可獲取姿態(tài)矩陣R。

在主手遙操作機(jī)器人工作的過程中， 如果在靠近目標(biāo)件時(shí)打開絕對模式連接主手， 從手移動到目標(biāo)姿態(tài)方向時(shí)可能會觸碰到目標(biāo)件， 造成誤差或者損壞從手和目標(biāo)件。 因此設(shè)計(jì)了相對模式， 在從手距離目標(biāo)件較近時(shí)使用， 同時(shí)也能更加便利地進(jìn)行作業(yè)。

在相對模式下， 主手循環(huán)掃描末端位姿信息， 計(jì)算出主手當(dāng)前姿態(tài)相對于上一次掃描步時(shí)姿態(tài)的變換矩陣， 傳遞給從手， 從手末端姿態(tài)相對于自身工作坐標(biāo)系運(yùn)動。 如圖5 所示， 從手末端姿態(tài)相對自身坐標(biāo)系｛P1XP1YP1ZP1｝運(yùn)動到｛P2XP2YP2ZP2｝。

圖5 相對模式姿態(tài)控制Fig.5 Posture control of relative mode

1.4 比例映射控制算法

在遙操作主手時(shí)， 需要把主手工作空間按照比例放大或縮小， 映射到從手上。 縮小時(shí)可以更好地控制誤差， 放大時(shí)能更快地到達(dá)目標(biāo)位置［19－20］。

在比例映射的算法中， 用比例縮放系數(shù)S表示主手工作空間的映射關(guān)系。 在位姿控制方法中， 比例縮放系數(shù)不影響姿態(tài)的變換， 只影響位置變換［21］。 因此在之前的位置算法中， 將主手世界坐標(biāo)系替換成主手映射坐標(biāo)系， 如圖6 所示。

圖6 主手比例映射坐標(biāo)系Fig.6 Master proportional mapping coordinate system

對應(yīng)公式替換如下：

式中：TM為主手獲取的位置矩陣。

通過比例映射， 可以更加靈活地操作機(jī)器人工作。

2 仿真實(shí)驗(yàn)

2.1 仿真軟件與設(shè)備

利用提出的位姿控制方法設(shè)計(jì)仿真實(shí)驗(yàn)， 使用軟件CoppeliaSim 和MATLAB 進(jìn)行聯(lián)合仿真， 同時(shí)連接外部力反饋設(shè)備Touch 手柄［22］進(jìn)行人機(jī)交互。

實(shí)驗(yàn)的主端設(shè)備為一臺Touch 手柄， 操作者操作手柄控制從端運(yùn)動。 從端為CoppeliaSim/V－REP 軟件內(nèi)的虛擬珞石機(jī)械臂， 如圖7 所示， 同時(shí)從端使用力反饋渲染框架CHAI3D 構(gòu)建環(huán)境。

圖7 仿真實(shí)驗(yàn)Fig.7 Simulation experiment： （a） master hand；（b） slave hand

圖8 控制信息流程Fig.8 Control information flow

實(shí)驗(yàn)中操作者需要通過手柄控制從端機(jī)械臂運(yùn)動， 同時(shí)記錄主從手位姿信息， 存儲數(shù)據(jù)后傳遞給MATLAB 進(jìn)行相關(guān)數(shù)據(jù)分析。 實(shí)驗(yàn)的始停由MATLAB的外部程序控制。

2.2 位置算法仿真實(shí)驗(yàn)驗(yàn)證

仿真實(shí)驗(yàn)中設(shè)計(jì)了3 個(gè)按鈕來控制Touch 手柄和虛擬機(jī)械臂的交互連接， 即“Connect”、 “Disconnect”和“Reset”。 “Connect” 和“Disconnect” 按鈕控制手柄的連接與斷開， 且自動計(jì)算相應(yīng)的矩陣變換關(guān)系?！癛eset” 按鈕重置為初次主從手連接交互時(shí)的狀態(tài)。

位置控制方法仿真驗(yàn)證手柄末端三維空間的移動是否能使虛擬機(jī)械臂達(dá)到同等的位移， 是否有偏差。實(shí)驗(yàn)過程中， 操作者操作手柄先進(jìn)行三維空間近似圓軌跡的移動， 完成一輪后再進(jìn)行近似直線軌跡的移動。 過程中分別記錄存儲手柄和虛擬機(jī)械臂末端的位置信息， 傳遞給MATLAB 進(jìn)行數(shù)據(jù)處理和分析。 程序設(shè)計(jì)手柄位置曲線用虛線表示， 從手位置曲線用實(shí)線表示。 實(shí)驗(yàn)結(jié)果如圖9 所示， 分別為坐標(biāo)信息中X、Y、Z數(shù)據(jù)隨時(shí)間的變化曲線。 圖中A—B階段表示手柄走近似圓軌跡的位置曲線，C—D階段是手柄走近似直軌軌跡的位置曲線。

圖9 手柄與機(jī)械臂位置坐標(biāo)Fig.9 Position coordinates of handle and manipulator： （a） X－axis； （b） Y－axis； （c） Z－axis

由圖9 可知： 2 條曲線軌跡相同， 可以通過平移使2 條曲線完全重合。 這表明操作者通過控制手柄移動可以使機(jī)械臂進(jìn)行相同位移， 手柄移動軌跡可以完美映射到機(jī)械臂中； 此算法能夠達(dá)到使用要求， 主手可以準(zhǔn)確地控制從手進(jìn)行相應(yīng)位移， 實(shí)現(xiàn)主手工作空 間對從手工作空間的覆蓋。

2.3 比例映射算法仿真實(shí)驗(yàn)驗(yàn)證

采用比例映射算法仿真驗(yàn)證手柄末端的位移是否能按比例映射到機(jī)械臂上。 仿真實(shí)驗(yàn)設(shè)計(jì)一個(gè)輸入讀取的顯示窗口， 用于更改比例參數(shù)且顯示當(dāng)前參數(shù)，程序設(shè)計(jì)在斷開手柄連接后才可更改。 通過仿真實(shí)驗(yàn)設(shè)計(jì)和位置實(shí)驗(yàn)類似的軌跡， 操作機(jī)械臂移動到合適位置時(shí)， 斷開位置連接， 分別設(shè)置比例參數(shù)為2 和0.5 后連接手柄。 操作者操作手柄以近似圓的路徑移動， 再完成近似直線的路徑。 收集仿真實(shí)驗(yàn)過程中手柄坐標(biāo)信息和機(jī)械臂末端坐標(biāo)信息， 經(jīng)過數(shù)據(jù)處理得到實(shí)驗(yàn)結(jié)果曲線， 同樣用虛線表示主手位置， 實(shí)線表示從手位置。 圖10 所示為2 倍比例系數(shù)的主從手位置曲線， 圖11 所示為0.5 倍比例系數(shù)下主從手位置曲線。A—B階段表示操作者控制主手走近似圓軌跡的位置時(shí)間曲線，C—D階段為主手走近似直線軌跡的位置時(shí)間曲線。

圖10 2 倍比例系數(shù)下手柄與機(jī)械臂位置坐標(biāo)Fig.10 Position coordinates of handle and manipulator with 2 proportional coefficient： （a） X－axis； （b） Y－axis； （c） Z－axis

由圖10—11 可知：A—B階段曲線近似正弦曲線，C—D階段曲線近似直線。A—B階段機(jī)械臂末端位置曲線的波幅分別為手柄位置曲線的2 倍和0.5倍； 而C—D階段機(jī)械臂末端位置曲線的斜率也分別是手柄位置曲線的2 倍和0.5 倍。 由此表明手柄的位移可以有效地按比例映射到機(jī)械臂上， 通過比例參數(shù)可以放大或者縮小主手的操作。

2.4 姿態(tài)算法仿真實(shí)驗(yàn)驗(yàn)證

姿態(tài)控制方法有2 種模式： 絕對模式和相對模式。 絕對模式是傳遞手柄的姿態(tài)矩陣給機(jī)械臂， 手柄和機(jī)械臂末端姿態(tài)坐標(biāo)形成一一對應(yīng)的關(guān)系。 相對模式手柄傳遞運(yùn)動信息給機(jī)械臂， 機(jī)械臂相對于自身坐標(biāo)系完成相應(yīng)的姿態(tài)變化。

程序設(shè)計(jì)了4 個(gè)按鈕， 分別控制絕對模式和相對模式下主從手的連接與中斷。 在絕對模式下， 機(jī)械臂末端運(yùn)行到目標(biāo)姿態(tài)坐標(biāo)。 在相對模式下， 機(jī)械臂進(jìn)行和手柄一樣的姿態(tài)變化， 機(jī)械臂和手柄的初始姿態(tài)坐標(biāo)不一定相同， 但是進(jìn)行了相同姿態(tài)變化。

實(shí)驗(yàn)設(shè)計(jì)操作者操作手柄進(jìn)行3 個(gè)姿態(tài)方向的旋轉(zhuǎn)運(yùn)動， 先進(jìn)行絕對模式的連接， 再完成相對模式的驗(yàn)證。 記錄手柄和機(jī)械臂末端的姿態(tài)信息， 傳遞給MATLAB 進(jìn)行數(shù)據(jù)分析與處理。 程序設(shè)計(jì)手柄姿態(tài)信息由虛線表示， 機(jī)械臂姿態(tài)信息由實(shí)線表示， 圖中標(biāo)記A—B階段為絕對模式，C—D階段為相對模式，實(shí)驗(yàn)結(jié)果如圖12 所示。 姿態(tài)的角度可以用多種表示方法， 由于CoppeliaSim 規(guī)定的歐拉角函數(shù)的差異和機(jī)械臂與手柄結(jié)構(gòu)尺寸等不一致， 仿真實(shí)驗(yàn)使用Yaw－Pitch－Roll 旋轉(zhuǎn)角度表示［23］。

圖12 手柄與機(jī)械臂姿態(tài)角度對比Fig.12 Posture angle comparison of the handle and manipulator： （a） yaw； （b） pitch； （c） roll

由圖12 可知： 絕對模式下2 條曲線重合， 表明手柄能夠準(zhǔn)確控制機(jī)械臂的方位； 相對模式下2 條曲線初始角度不同， 但曲線的軌跡相同， 通過平移可以使2 條曲線重合。 這表明相對模式下手柄可以把自身的姿態(tài)運(yùn)動信息傳遞給機(jī)械臂， 使它能夠進(jìn)行同樣的姿態(tài)變化。 實(shí)驗(yàn)驗(yàn)證了絕對模式和相對模式下姿態(tài)控制的功能。

3 結(jié)語

文中設(shè)計(jì)了一種位置區(qū)域覆蓋算法， 可通過多次連接， 使主手工作空間覆蓋從手工作空間。 同時(shí)為解決主從手工作時(shí)姿態(tài)控制的便利性和安全性問題， 設(shè)計(jì)了一種相對姿態(tài)控制方法。 利用仿真軟件Coppeli?aSim 搭建仿真實(shí)驗(yàn)平臺， 同時(shí)連接外部設(shè)備Touch 手柄控制虛擬機(jī)械臂進(jìn)行驗(yàn)證。 利用MATLAB 對實(shí)驗(yàn)位姿數(shù)據(jù)進(jìn)行處理分析且生成相應(yīng)曲線。 結(jié)果表明：設(shè)計(jì)的控制方法可以達(dá)到預(yù)期目的， 能夠準(zhǔn)確控制主從手運(yùn)動， 實(shí)現(xiàn)主手工作空間覆蓋從手。