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

李強， 曾宇飛， 武圣杰， 張華良

（1.沈陽工業大學機械工程學院， 遼寧沈陽 110870；2.中國科學院沈陽自動化研究所， 遼寧沈陽 110017）

0 前言

隨著科技的發展， 人們的社會生產活動越來越智能化， 機器人技術發展迅速［1］。 然而由于相關技術的限制， 全自主式的智能機器人難以實現， 遙操作機器人成為一種可行的方法［2］。

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

1 位姿控制方法

文中設計的控制方法分為3 個部分： 區域覆蓋位置控制、 相對姿態控制、 比例映射控制。

在遙操控機器人中， 主手和從手是一一對應的關系， 在循環掃描過程中， 主手將位姿信息傳遞給從手， 從手追蹤移動到該位姿［9］。 文中的位置控制方法將主從手一一對應關系轉換為多次映射關系， 以達到主手工作空間覆蓋目的。 同時， 為了更安全和便利地操作主從手進行作業， 設計了相對姿態方法和比例映射方法。

1.1 主手運動學建模

文中設計的控制方法主要是完成位置和姿態的控制， 使用Touch 手柄達成目標。 手柄結構簡圖如圖1所示。

使用改進D－H 參數法建立主操作手坐標系［10］，如圖1 所示， 其中｛O0X0Y0Z0｝ 為主手的世界坐標系（主手基坐標系）。 主手后3 個關節為姿態坐標系， 滿足三軸交于一點［11］， 末端點位于｛O5X5Y5Z5｝坐標系原點。

建立運動學方程為

連桿長度、 連桿扭角、 連桿距離等參數已知， 通過獲取各個連桿夾角即可算出運動學方程。

1.2 區域覆蓋位置控制算法

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

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

算法實現原理如圖3 所示。 圖中｛O0X0Y0Z0｝坐標系為從手的世界坐標系， ｛O1X1Y1Z1｝和｛O2X2Y2Z2｝為多次映射建立的主手世界坐標系， ｛P1XP1YP1ZP1｝和｛P2XP2YP2ZP2｝是對應主手世界坐標系下建立的工作坐標系。

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

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

再次斷開連接后， 記錄從手最后位置P2。 同理建立第三次主從手連接， 以點P2為主手末端點建立主手世界坐標系｛O2X2Y2Z2｝， 將P2在｛O2X2Y2Z2｝坐標系下的坐標轉化為從手世界坐標系｛O0X0Y0Z0｝下的坐標， 即可控制從手運動。 通過重復變換， 可以實現位置的覆蓋算法。

所以姿態矩陣可以分解為姿態矩陣和位置矩陣：

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

1.3 相對姿態控制算法

機器人的姿態是指機器人在空間中的方位。 在串聯型主手中， 由于各關節是串聯連接， 位姿不分離，主手位置改變時主手的姿態也會改變， 在操作過程中容易引起誤差［15－16］， 因此文中設計的控制方法中，從手的位置和姿態分別由單獨的開關控制。 當斷開主從手姿態連接時， 主手停止傳遞姿態信息給從手， 以此規避位姿不分離所造成的影響。

姿態控制分為2 個模式： 絕對模式和相對模式。2 個模式通過單獨的開關來控制連接與中斷。 在離目標件較遠的安全位置時， 可使用絕對模式， 復位主從手姿態角度； 在離目標件較近的位置時， 使用相對模式， 能更加便利地進行作業。

當絕對模式連接時， 主手的世界坐標系會與從手的世界坐標系重合， 主從手是一一對應的映射關系［17］， 如圖4 所示。

主手世界坐標系｛O1X1Y1Z1｝ 與從手世界坐標系｛O0X0Y0Z0｝ 重合， 主手通過獲取末端點P1姿態信息傳遞給從手， 控制從手的姿態方位［18］。

通過位姿矩陣可獲取姿態矩陣R。

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

在相對模式下， 主手循環掃描末端位姿信息， 計算出主手當前姿態相對于上一次掃描步時姿態的變換矩陣， 傳遞給從手， 從手末端姿態相對于自身工作坐標系運動。 如圖5 所示， 從手末端姿態相對自身坐標系｛P1XP1YP1ZP1｝運動到｛P2XP2YP2ZP2｝。

圖5 相對模式姿態控制Fig.5 Posture control of relative mode

1.4 比例映射控制算法

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

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

圖6 主手比例映射坐標系Fig.6 Master proportional mapping coordinate system

對應公式替換如下：

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

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

2 仿真實驗

2.1 仿真軟件與設備

利用提出的位姿控制方法設計仿真實驗， 使用軟件CoppeliaSim 和MATLAB 進行聯合仿真， 同時連接外部力反饋設備Touch 手柄［22］進行人機交互。

實驗的主端設備為一臺Touch 手柄， 操作者操作手柄控制從端運動。 從端為CoppeliaSim/V－REP 軟件內的虛擬珞石機械臂， 如圖7 所示， 同時從端使用力反饋渲染框架CHAI3D 構建環境。

圖7 仿真實驗Fig.7 Simulation experiment： （a） master hand；（b） slave hand

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

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

2.2 位置算法仿真實驗驗證

仿真實驗中設計了3 個按鈕來控制Touch 手柄和虛擬機械臂的交互連接， 即“Connect”、 “Disconnect”和“Reset”。 “Connect” 和“Disconnect” 按鈕控制手柄的連接與斷開， 且自動計算相應的矩陣變換關系。“Reset” 按鈕重置為初次主從手連接交互時的狀態。

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

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

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

2.3 比例映射算法仿真實驗驗證

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

圖10 2 倍比例系數下手柄與機械臂位置坐標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階段機械臂末端位置曲線的波幅分別為手柄位置曲線的2 倍和0.5倍； 而C—D階段機械臂末端位置曲線的斜率也分別是手柄位置曲線的2 倍和0.5 倍。 由此表明手柄的位移可以有效地按比例映射到機械臂上， 通過比例參數可以放大或者縮小主手的操作。

2.4 姿態算法仿真實驗驗證

姿態控制方法有2 種模式： 絕對模式和相對模式。 絕對模式是傳遞手柄的姿態矩陣給機械臂， 手柄和機械臂末端姿態坐標形成一一對應的關系。 相對模式手柄傳遞運動信息給機械臂， 機械臂相對于自身坐標系完成相應的姿態變化。

程序設計了4 個按鈕， 分別控制絕對模式和相對模式下主從手的連接與中斷。 在絕對模式下， 機械臂末端運行到目標姿態坐標。 在相對模式下， 機械臂進行和手柄一樣的姿態變化， 機械臂和手柄的初始姿態坐標不一定相同， 但是進行了相同姿態變化。

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

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

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

3 結語

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