基于六維鼠標的機器人位姿控制研究

祝潤澤,黃昕,高燕

(廣州機械科學研究院有限公司中央研究所，廣東廣州 510700)

0 前言

隨著制造業(yè)轉型升級，工業(yè)機器人逐漸演變成普通工業(yè)消費品，大量中小企業(yè)在長尾行業(yè)開始引入機器人來替代人工。在如今不斷追求短平快的市場環(huán)境下，產品換線頻率逐漸升高，固定單品的凈生產時間逐漸下降，這使得機器人的程序示教時間在整體機器人應用時間中的占比逐漸上升，高昂的維護時間成本成了阻礙機器人應用的主要因素。機器人的示教編程主要分為示教器按鍵示教和直接示教。不同于示教器按鍵示教中對物理按鍵的點擊操作，在直接示教中，操作人員可通過直接拖拽機器人本體的方式移動機器人末端以實現(xiàn)示教目的。當通過按鍵操作時，操作人員坐標系與機器人工作坐標系相互獨立。在直接示教的方式中，由于操作人員直接與機器人互動，操作坐標系與機器人工作坐標系融合，對機器人的控制更加直觀。由于這種差異，直接示教的方式在降低對操作人員水平要求的同時也大大提高了示教效率。CHOI和LEE對80名無經驗操作人員進行了實驗，發(fā)現(xiàn)直接示教的方式相對于示教器按鍵操作，其示教時間成本降低1/2，同一任務直接示教的方式在初次操作時所需的時間與按鍵操作經過多次練習后相當。由此可見，直接示教是降低示教時間的關鍵。

1 相關工作

機器人的直接示教可以通過多種方式實現(xiàn)。針對一些不具備直接示教功能的封閉機器人系統(tǒng)，直接示教通常通過增加外部設備實現(xiàn)。劉昆等人通過增加外部力/力矩傳感器，收集使用者施加在機器人上的力信號,實現(xiàn)對機器人位姿的控制。通過力控的方式雖然可以實現(xiàn)直接示教，但是受限于力控的帶寬限制，示教時通常存在響應慢的問題，并且六維力傳感器價格昂貴，整機成本缺少競爭力。CHOI和KIM設計了一種簡單的采集力/力矩方向信號的裝置，通過安裝于機器人末端實現(xiàn)對機器人的直接操作。由于此設備在每個空間維度僅能提供on-off信號，很難對機器人的位置進行連續(xù)精細控制。除了在末端增加設備之外，通過對機器人各個關節(jié)的力矩進行檢測也可以實現(xiàn)對關節(jié)的拖動示教。在各個關節(jié)增加力矩傳感器可以實現(xiàn)較好的拖動效果，但是成本高昂，在目前的國內市場非常不適用?；趧恿W模型辨識實現(xiàn)的機器人零力控制不需要增加任何外部傳感器，是實現(xiàn)拖動示教的首選方式。在應用時，首先需要對機器人的動力學模型進行辨識，在運動過程中對重力以及摩擦力進行補償后，構建機器人各軸的期望力矩，從而實現(xiàn)零力控制。由于機器人結構影響，實際進行拖動機器人末端時，受限于機器人各軸不等長的力臂，作用在機器人末端的控制力很難實現(xiàn)對機器人各軸的均勻拖動。以六軸機器人為例，離基座更近的一、二、三軸在拖動時往往比腕關節(jié)的3個軸更加敏感，在實際使用中同樣存在很難對末端位置進行精準控制的問題。為使操作坐標系與工作坐標系統(tǒng)一，SUGITA和ITAYA設計了一種示教外設，操作者直接手動操作此外設在加工表面進行示教，用直接操作其他設備的方式取代直接操作機器人，所有示教點位均由該設備記錄，機器人僅作為后端加工執(zhí)行單元。

在實際應用中，直接示教的方法還需要工作軌跡規(guī)劃的支持。陳宇鵬等通過非均勻B樣條曲線對直接示教的軌跡進行擬合，從而實現(xiàn)平順的直接示教軌跡再現(xiàn)。

本文作者提出的基于六維鼠標的機器人位姿控制在成本上相較于力傳感器更低，相對于運動學拖動示教可以更加直觀地控制機器人末端位姿，在實際運動時軌跡由專用控制器生成，更有利于獲得均勻平順的離散點位，更適合直接示教的執(zhí)行復現(xiàn)。

2 六維鼠標

圖1所示為此方案中使用的六維鼠標,是來自3Dconnexion公司的SpaceMouse compact產品，該產品已被廣泛應用于CAD建模。六維鼠標輸出、、、、、六個維度的移動量信號。在、、方向提供±1.5 mm的檢測位移量，在、、方向提供±6°的旋轉量。該產品通過USB進行連接，交互數據包大小為7 B，其中第一字節(jié)指明了當前數據包的數據類型，其有效值為1、2、3，分別對應移動數據、旋轉數據以及按鍵數據,如表1—表4所示。移動數據包和旋轉數據包均包含6 B有效數據，由3個INT_16類型數據構成。對于按鍵數據，數據包有效數據僅1 B，通過兩位數據表述實際硬件中的2個按鍵狀態(tài)。

圖 1 3Dconnexion SpaceMouse

表1 移動數據

表2 旋轉數據

表3 按鍵數據1

表4 按鍵數據2

3 系統(tǒng)結構

基于六維鼠標的位姿控制主要由以下系統(tǒng)結構構成。主要流程包含鼠標內部的傳感器數據獲取、機器人控制器對鼠標數據的處理、坐標系轉換、軌跡控制以及到關節(jié)空間的轉換、伺服電機控制。其中，鼠標的傳感器數據獲取由鼠標產品提供，通過USB進行數據對接；伺服控制采用通用商業(yè)伺服，采用位置控制模式進行開環(huán)控制。本文作者著重進行機器人控制器端的設計與架構。圖2所示為完整的系統(tǒng)結構流程。

圖2 系統(tǒng)結構

其中，為鼠標原始數據，其結構為

=[]

為笛卡爾空間下的控制軌跡目標點:

=[]

式中：、、為遵循--原則的歐拉角;為關節(jié)角度，表示為

4 坐標系轉換

坐標系轉換是使用六維鼠標數據控制機器人的必要流程之一。六維鼠標產品在出廠時已通過內部傳感器校準等方式確定產品信號坐標系，其坐標系定義如圖3所示。當產品被安裝于機器人末端并用于控制機器人工具末端運動時，信號坐標系便與控制坐標系存在差異，兩者間的關系定義如下：

圖3 裝于機器人末端的鼠標

通過增加裝配結構校準釘的方式，可獲得：

經計算，得：

5 軌跡控制

為保證機器人的平順運行，經過轉換的六維鼠標數據需要經過控制器內部規(guī)劃，生成滿足機器人關節(jié)加加速度、加速度以及速度限制的運動軌跡。在軌跡控制方面，采用一種改型3層PID運動控制器實現(xiàn)，該控制器包含加速度環(huán)、速度環(huán)以及位置控制環(huán)。每層控制器均采用完整PID進行控制，且在每層控制環(huán)的輸入端增加極值限制，通過限制輸入的方式保證輸出的加加速度、加速度以及速度均在允許范圍之內。該控制器意在借助PID控制器生成連續(xù)平順的軌跡，所以在最內環(huán)加加速度環(huán)，控制系統(tǒng)被設置為直接控制系統(tǒng)，即系統(tǒng)的系數為，在后續(xù)測試中，=1。

圖4所示為該控制器的基本結構，在3層運動學PID控制器的基礎上，調整位置環(huán)輸入以及速度環(huán)極值限制，實現(xiàn)基于六維鼠標數據的機器人速度控制。來自六維鼠標的數據首先通過一個極值過濾，將連續(xù)的控制數據根據正負轉換成為Bang-Bang控制的信號，控制信號為一個位置上的極大值,極大值通過系數調整。在位置環(huán)采用Bang-Bang控制信號，主要實現(xiàn)調整運動方向的功能。在輸入位置控制環(huán)的同時，六維鼠標的原始數據取絕對值以獲取移動量的矢量信息，結合當前系統(tǒng)允許的最大速度，其乘積作為系數傳入，再調整符號，結合設置的最大速度和當前六維鼠標大小的信號降作為系統(tǒng)中新的最大速度和最小速度限制，確保速度環(huán)的輸入在控制速度范圍內。

圖4 軌跡控制結構

6 機器人逆解

經過上述規(guī)劃控制器轉換后，其控制結果處于笛卡爾空間中。此時所有笛卡爾坐標系中的點位需經過機器人逆解計算，將獲得的對應的關節(jié)位置作為機器人各軸的目標點位使用。通過對不同機械機構建立逆解算法，結合六維鼠標的位姿控制，可以輕松適配不同機器人機型，降低算法應用的復雜度。

以通用六軸機器人為例，笛卡爾坐標至關節(jié)空間的轉換可以通過幾何法實現(xiàn)。由于機器人末端腕關節(jié)4、5、6軸的旋轉軸在結構上十字相交，當法蘭盤的笛卡爾位置已知時，可通過沿末端軸方向反算腕關節(jié)交點位置。交點位置僅受1、2、3軸影響，此時根據交點位置與坐標系原點在平面上的投影即可獲得1軸角度，通過2、3軸連桿長度和交點與坐標系原點的距離可以計算出2、3軸的角度。根據計算所獲的前三軸數據，通過正解可獲得在腕關節(jié)處4軸的旋轉軸方向，結合法蘭盤位置，即可計算4、5、6軸關節(jié)的角度。

7 上機實驗

該項目控制結構使用國機智能SR07L機器人進行上機應用實驗。算法部署基于國機智能自行研發(fā)的機器人控制器，采用經過裁剪的Linux系統(tǒng)，借助系統(tǒng)原生的工具校準算法以及機器人正逆解實現(xiàn)。如圖5所示，六維鼠標通過3D打印的連接器安裝于機器人末端，通過在機器人法蘭盤位置的法蘭進一步固定操作工具。

圖5 SR07L機器人

圖6所示為基于六維鼠標的控制效果。圖中縱坐標為歸一化后的鼠標位移量以及機器人笛卡爾空間內的速度，可知機器人位置實現(xiàn)了對六維鼠標指令的跟蹤，速度可隨鼠標指令量調整。

圖6 實驗結果

8 結束語

針對更加直觀的機器人示教方式，本文作者提出了一種基于六維鼠標的機器人位姿控制方式，涵蓋了從鼠標數據獲取、坐標系轉換、軌跡控制到機器人逆解直至伺服控制的全部流程。在實際上機實驗中，利用該控制流程控制機器人末端位姿，實現(xiàn)了對鼠標指令的有效跟蹤。這種直接示教的方法使用了獨立外部

設備對位姿進行直接控制，因此可以通過替換機器人逆解直接對不同種類的機器人進行適配。因為不依賴于任何動力學模型，在實際裝機使用時可省去動力學模型辨識的流程。從成本上考慮，六維鼠標的物料成本遠低于六維力傳感器。盡管相較于無傳感器的動力學，拖動示教成本有所增加，但考慮直接對末端進行精準位姿控制，基于六維鼠標的方式依然比動力學拖動示教有更好的效果，且其直接示教部署難度更低、速度更快，在實際量產使用中有著更好的應用前景。