面向空間機械臂操作任務的模仿學習策略

李重陽，蔣再男，劉 宏，蔡鶴皋

(機器人技術與系統國家重點實驗室(哈爾濱工業大學)，哈爾濱 150001)

近年來，隨著空間技術和機械臂技術的發展，在空間探索任務中，空間機械臂的應用越來越廣泛.利用空間機械臂輔助或代替航天員進行空間作業，不僅大大降低了航天員出艙活動的機率和風險，而且可以提高空間探索的效率[1-3].2016年9月15日，天宮二號空間機械臂搭載發射升空，并成功完成了所有規劃實驗，這為后續空間站機械臂的設計和控制積累了寶貴的經驗[4].實驗過程中，機械臂需完成一系列維護維修實例，如利用電動工具旋擰螺釘、拔插電連接器、拆除多層以及抓取漂浮小球等，這些操作實例基本覆蓋了已知的在軌維護維修任務中將會面對的所有問題.如何控制機械臂完成以上任務且能克服環境變化的干擾，將是必須面對的挑戰.

傳統的預編程離線軌跡的方法雖然能夠在結構化的空間中完成上述任務[5]，但是環境適應性較差，一旦位置發生改變，則不能完成任務.雖然與視覺伺服[6]、激光引導[7]等方法相結合后，可提高一定的環境適應性，但是當與預知工況差別較大時，仍難以完成任務.同時，該方法需要對每一項任務單獨編程，即使對同一類任務也需如此，大大增加了操作者的工作量.

針對上述問題，模仿學習策略是一種很好的解決方案[8-10].該策略是一種技能學習策略，能夠根據操作者提供的某類任務的運動學實例，構建該任務的運動模型，然后根據當前環境回歸獲得適合當前任務的軌跡.該策略能夠有效地克服環境變化帶來的干擾[11-13]，且針對其他類別的任務，只需要輸入對應的運動學實例即可，大大簡化了操作.但該策略若直接應用在空間機械臂操作中仍存在一些問題.該策略主要關注的是機械臂能夠克服環境變化的干擾，并方便快速地生成當前任務軌跡；但機械臂執行生成的軌跡時，沒有考慮關節期望力矩產生的波動以及運動是否平滑等因素.而空間機械臂大的力矩波動很有可能造成控制不穩定，甚至導致任務失敗[1].

本文提出了一種基于動力學約束的模仿學習策略，相對于傳統的模仿學習策略，該策略加入了基于機械臂動力學約束的優化，可使機械臂執行任務時的力矩波動和能量損耗均減小，提高控制器的穩定性.

1 方法概述

基于動力學約束的模仿學習策略是一種技能學習策略，相對以往的模仿學習策略，該策略重新設計了機械臂控制器，增加了針對自主生成軌跡的基于動力學約束的優化.基于動力學約束的模仿學習策略主要分為以下兩個階段，如圖1所示：

圖1 基于動力學約束的模仿學習策略

Fig.1 Learning strategy from demonstration based on dynamics constraint

第一階段為基于高斯過程的模仿學習.首先，該階段需要對任務軌跡進行形式化；然后，應用高斯過程算法利用運動學實例對其進行訓練，獲得當前任務的運動模型；最后，利用訓練好的模型以及全局相機和傳感器采集到的當前環境狀態信息,回歸出適合當前場景的笛卡爾期望軌跡的概率分布；

第二階段為基于動力學約束的機械臂關節控制器設計.該階段首先將第一階段中獲得的笛卡爾期望軌跡分布映射到關節空間，然后根據最優控制理論設計機械臂關節控制器，該控制器以機械臂動力學為約束條件，以關節期望力矩為輸出.

2 基于高斯過程的模仿學習

與以往的模仿學習策略不同，基于高斯過程的模仿學習策略主要是利用高斯過程算法[14]對運動模型進行訓練，并利用高斯過程回歸獲得適合當前任務場景的笛卡爾軌跡分布，使所得結果為一個概率模型，而不僅僅是一條期望軌跡，這為后續的優化提供了條件.

在模仿學習階段，首先需要對機械的任務軌跡進行建模.依據動態系統理論，將任務軌跡形式化成一階自治常微分方程的形式，這種形式化方法可使模型在存在空間擾動時，表現出較強的魯棒性[15].具體表達形式為

(1)

式中：f(·)：n→n為一個連續且可微的非線性微分方程，該方程具有唯一的平衡點En表示動態系統的噪聲，在本文中主要指來自傳感器測量的噪聲，并假設該噪聲為高斯噪聲，其形式為E～N(0,σ2I) ；ξn為輸入的狀態變量，可根據實際情況選擇(例如，ξ可以是機械臂的關節角度、末端位姿以及末端速度等)，不同的狀態變量對應的微分方程的階數也不同.根據實際情況，本文選擇機械臂的末端位姿為輸入的狀態變量ξ=[x,y,z,Rx,Ry,Rz]，則可得

(2)

式中，σf和l均為該核函數的參數，可通過最小化負對數邊界似然獲得.

由式(2)可知，運動模型主要描述了當前任務中機械臂速度與位置之間的關系，即根據任務構建出一個速度場.在速度場中的任意位置，均可通過高斯過程回歸獲得該位置的期望速度：

在實際應用中，利用t時刻的位置信息可回歸出t時刻的期望速度，從而可以計算出t+1時刻期望的位置：

(3)

在實際應用中，由于該策略的運動學實例均是根據操作者控制機械臂運動的軌跡計算的，不可避免地帶有操作者的一些操作特征.例如，在目標點附近時，操作者往往會反復來回移動操作手柄，以保證對準目標位置.這些操作將會造成生成的笛卡爾期望軌跡收斂時間過長，甚至引起發散.

因此，本文基于文獻[17]改進了速度場方法，建立了一個線性速度場.在迭代式(3)的過程中，如果與目標點的距離<δ，則軌跡生成不再繼續迭代式(3)，而是根據線性速度場計算速度及其分布.同樣以沿X軸方向為例，令xg表示沿X軸方向的目標位置.一旦存在時刻t，使得|xt-xg|≤δ，則進入線性速度場范圍，速度計算方法為

3 基于動力學約束的機械臂關節控制器設計

基于高斯過程的模仿學習策略可根據機械臂及環境的狀態，自主生成對應的笛卡爾任務分布，具有較強的環境適應性.但是，由于生成的軌跡分布均參考來自操作者的運動學實例，受人類生理特性影響，運動學實例本身就可能不夠平滑，尤其經過較長時間工作后，操作者肌肉疲勞，這種現象更加明顯；因此，直接將獲得的期望軌跡分布均值用于機械臂控制，可能會造成關節控制力矩波動過大，甚至控制不穩定.

針對上述問題，提出了一種基于動力學約束的機械臂關節控制器.該控制器旨在保證成功完成任務的同時，減小關節輸出力矩波動和能量消耗，使機械臂的運動更加平滑.

對于一項任務，一般均有多條不同軌跡能滿足要求，即當軌跡位于一定的置信區間內，均可認為能夠滿足任務需求.而上節中的軌跡分布可用于計算該置信區間.因此，將控制器設計成線性二次跟蹤型(LQT)的形式：

(s(t)-s*(t))+uT(t)·R·u(t))+

(s(tf)-s*(tf))T·Qf·(s(tf)-s*(tf)),

(4)

式中：∑t為t時刻期望軌跡分布的協方差，Q為定值.引入馬氏距離，可以理解為當∑t大時，期望軌跡的不確定性高，機械臂控制器可以有更大空間調整，優化將傾向于減少輸出力矩，反之亦然.

本文采用微分動態規劃方法[18]對式(4)問題進行求解.帶偏置的LQT問題的最優反饋控制律可寫成如下形式：

u*(t)=-R-1·BT(t)·(M(t)·s(t)+p(t)).

其中，M(t)和p(t)均可通過Ricatti遞歸進行求解,

(5)

M(t)和p(t)可通過對式(5)進行數值積分獲得.參數Q、R和Qf需要人為調節，以使控制器達到更好的效果.

但是，該控制器工作在機械臂關節空間，而基于高斯過程的模仿學習策略的結果是笛卡爾空間的軌跡分布，因此需要對其進行映射.該分布的均值映射較為簡單，直接利用逆運動學進行映射即可.而協方差映射需要從速度分布映射入手.由于笛卡爾空間的速度和關節空間的速度在瞬時為線性關系

4 實驗驗證

為驗證策略的可行性，本文利用天宮二號空間機械臂進行實驗驗證.該機械臂為六自由度輕型機械臂，Denavit-Hartenberg(D-H)參數如表1所示，慣量參數如表2所示,其坐標系如圖2所示.其中，慣量為相對于質心測量獲得，質心位置為相對于D-H坐標系測量獲得.為簡化計算，在計算慣量參數時，將靈巧手和機械臂末端劃分成一個整體.

表1 D-H參數

表2 慣量參數

本文利用空間機械臂定位螺釘的實驗驗證上述策略，如圖3所示.實驗場所位于天宮二號空間實驗室內，環境為空間微重力環境.該實驗是天宮二號機械臂利用電動工具旋擰螺釘任務的重要組成部分.

圖2 天宮二號機械臂坐標系

圖3 電動工具定位螺釘實驗

實驗開始時，空間機械臂已經完成了電動工具的抓取，需要通過基于動力學約束的模仿學習策略引導機械臂到達螺釘上方20 mm處的預旋擰位置，做好旋擰準備.具體執行步驟如下：

Step1:操作者操作CyberForce力反饋手柄控制機械臂完成該任務，并記錄末端軌跡.為兼顧結果的泛化能力和操作的便捷性，本實驗中采集同一操作者的3組操作軌跡，并據此生成該任務的運動學實例.為驗證算法的泛化能力，3組操作軌跡均為直接記錄，未經過挑選.為方便后續計算，本文在螺釘上方20 mm處的目標位置處建立了目標坐標系，如圖3所示，并將運動學實例在該坐標系下表示，如圖4所示：

Step2:利用全局相機以及機械臂自身傳感器測量機械臂末端位姿與目標位姿，并在目標坐標系下表示，具體數值為[99.47 mm,-103.35 mm,-2.72 mm,0°,0°,0°].然后，利用基于高斯過程的模仿學習策略生成期望的適合該任務的笛卡爾軌跡分布(目標坐標系下表示)，如圖5所示.其中虛線為期望的笛卡爾軌跡均值，灰色區域為置信區間，該區間根據置信度和分布的協方差計算得到，本文中設置置信度為68.27%(3σ原則).

圖4 運動學實例軌跡

圖5 期望的笛卡爾軌跡分布

由于機械臂和操作平臺均為在軌重新組裝，所以地面記錄的位置并不準確.實際實驗中發現，因為艙體膨脹以及操作臺支架存在縫隙，實際偏差遠遠超出了電動工具的容差范圍,但是在利用基于動力學約束的模仿學習策略時仍能完成任務，體現了該策略具有較好的環境適應性.

Step3:設計基于動力學約束的機械臂關節控制器，生成期望的關節驅動力矩，并指導空間機械臂運行.初始狀態下，機械臂實測關節角度為[79.85°,-9.44°,-76.48°,-83.47°,-81.54°,-10.46°].將該控制器與計算力矩控制器對比，關節控制力矩、關節角加速度和關節速度如圖6～8所示.

由圖6可以看出，與傳統的計算力矩控制器相比，基于動力學約束的控制器能產生更加平滑的控制力矩.尤其是負載較大的前3個關節，效果更加明顯.以第1關節為例，基于動力學約束的控制器產生的控制力矩的峰-峰值相對于計算力矩控制器減小了45%以上，波峰數也由原來的5個減小到了3個，減少了40%，并且有效平滑了尖峰.

由圖7可以看出，應用基于動力學約束的控制器可以產生更加平滑的關節角加速度.其變化趨勢與關節力矩類似，各個關節角加速度的峰-峰值和波峰數均有明顯減小，尤其是關節6，峰-峰值由1.077 8減小到0.405 2，減小了62%，波峰數由5個減小到3個，減小了40%.應用基于動力學約束的控制器時，關節角加速度沒有尖峰，運動更加平滑.

如由8可以看出，應用基于動力學約束的控制器可使機械臂運動過程中速度的波峰數量減少，且波動的峰-峰值也會隨之減小.

能耗是衡量空間機械臂性能的重要指標，本實驗中的能耗對比如圖9所示.由圖9可知，應用基于動力學約束的控制器可使機械臂運行能耗明顯降低，以該任務為例，相比于應用計算力矩控制器，基于動力學約束的控制器的能耗可減少31%.

圖6 機械臂關節控制力矩

圖7 機械臂關節角加速度

圖8 機械臂關節角速度

圖9 機械臂能量消耗

圖10為應用基于動力學約束控制器時，機械臂末端軌跡在置信區間中的位置.

圖10 實際笛卡爾軌跡

由圖10可以看出，末端軌跡均位于依據3σ原則建立的置信區間內，能夠滿足任務需求.

4 結 論

1)本文面向空間機械臂操作任務，提出了一種基于動力學約束的模仿學習策略.該策略主要分為兩個階段，第一階段為基于高斯過程的模仿學習；第二階段為基于動力學約束的機械臂控制器設計.與傳統的模仿學習加計算力矩控制器的策略相比，該策略不僅具有操作便捷且克服空間擾動的優點，還可以有效降低關節力矩波動,同時減小能量消耗.

2)基于高斯過程的模仿學習階段引入改進的速度場方法，克服了臨近目標位置時笛卡爾期望軌跡收斂時間過長，甚至引起發散的問題.

3)在基于動力學約束的機械臂控制器設計階段引入馬氏范數，充分考慮了期望軌跡分布的不確定性，使該控制器的調整優化更符合任務需求.

4)為驗證該方法的有效性，設計了機械臂定位螺釘的實驗.實驗結果表明，該策略與傳統模仿學習加計算力矩控制的策略相比，大負載關節力矩波動的峰-峰值可減少45%，波峰數可減少40%，能耗可減少31%，且使得關節力矩、角加速度以及角速度更加平滑.未來該策略可以嘗試應用于接觸任務和自由漂浮基座空間機械臂.