面向空間機(jī)械臂操作任務(wù)的模仿學(xué)習(xí)策略

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

(機(jī)器人技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室(哈爾濱工業(yè)大學(xué))，哈爾濱 150001)

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

傳統(tǒng)的預(yù)編程離線軌跡的方法雖然能夠在結(jié)構(gòu)化的空間中完成上述任務(wù)[5]，但是環(huán)境適應(yīng)性較差，一旦位置發(fā)生改變，則不能完成任務(wù).雖然與視覺伺服[6]、激光引導(dǎo)[7]等方法相結(jié)合后，可提高一定的環(huán)境適應(yīng)性，但是當(dāng)與預(yù)知工況差別較大時(shí)，仍難以完成任務(wù).同時(shí)，該方法需要對(duì)每一項(xiàng)任務(wù)單獨(dú)編程，即使對(duì)同一類任務(wù)也需如此，大大增加了操作者的工作量.

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

本文提出了一種基于動(dòng)力學(xué)約束的模仿學(xué)習(xí)策略，相對(duì)于傳統(tǒng)的模仿學(xué)習(xí)策略，該策略加入了基于機(jī)械臂動(dòng)力學(xué)約束的優(yōu)化，可使機(jī)械臂執(zhí)行任務(wù)時(shí)的力矩波動(dòng)和能量損耗均減小，提高控制器的穩(wěn)定性.

1 方法概述

基于動(dòng)力學(xué)約束的模仿學(xué)習(xí)策略是一種技能學(xué)習(xí)策略，相對(duì)以往的模仿學(xué)習(xí)策略，該策略重新設(shè)計(jì)了機(jī)械臂控制器，增加了針對(duì)自主生成軌跡的基于動(dòng)力學(xué)約束的優(yōu)化.基于動(dòng)力學(xué)約束的模仿學(xué)習(xí)策略主要分為以下兩個(gè)階段，如圖1所示：

圖1 基于動(dòng)力學(xué)約束的模仿學(xué)習(xí)策略

Fig.1 Learning strategy from demonstration based on dynamics constraint

第一階段為基于高斯過程的模仿學(xué)習(xí).首先，該階段需要對(duì)任務(wù)軌跡進(jìn)行形式化；然后，應(yīng)用高斯過程算法利用運(yùn)動(dòng)學(xué)實(shí)例對(duì)其進(jìn)行訓(xùn)練，獲得當(dāng)前任務(wù)的運(yùn)動(dòng)模型；最后，利用訓(xùn)練好的模型以及全局相機(jī)和傳感器采集到的當(dāng)前環(huán)境狀態(tài)信息,回歸出適合當(dāng)前場(chǎng)景的笛卡爾期望軌跡的概率分布；

第二階段為基于動(dòng)力學(xué)約束的機(jī)械臂關(guān)節(jié)控制器設(shè)計(jì).該階段首先將第一階段中獲得的笛卡爾期望軌跡分布映射到關(guān)節(jié)空間，然后根據(jù)最優(yōu)控制理論設(shè)計(jì)機(jī)械臂關(guān)節(jié)控制器，該控制器以機(jī)械臂動(dòng)力學(xué)為約束條件，以關(guān)節(jié)期望力矩為輸出.

2 基于高斯過程的模仿學(xué)習(xí)

與以往的模仿學(xué)習(xí)策略不同，基于高斯過程的模仿學(xué)習(xí)策略主要是利用高斯過程算法[14]對(duì)運(yùn)動(dòng)模型進(jìn)行訓(xùn)練，并利用高斯過程回歸獲得適合當(dāng)前任務(wù)場(chǎng)景的笛卡爾軌跡分布，使所得結(jié)果為一個(gè)概率模型，而不僅僅是一條期望軌跡，這為后續(xù)的優(yōu)化提供了條件.

在模仿學(xué)習(xí)階段，首先需要對(duì)機(jī)械的任務(wù)軌跡進(jìn)行建模.依據(jù)動(dòng)態(tài)系統(tǒng)理論，將任務(wù)軌跡形式化成一階自治常微分方程的形式，這種形式化方法可使模型在存在空間擾動(dòng)時(shí)，表現(xiàn)出較強(qiáng)的魯棒性[15].具體表達(dá)形式為

(1)

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

(2)

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

由式(2)可知，運(yùn)動(dòng)模型主要描述了當(dāng)前任務(wù)中機(jī)械臂速度與位置之間的關(guān)系，即根據(jù)任務(wù)構(gòu)建出一個(gè)速度場(chǎng).在速度場(chǎng)中的任意位置，均可通過高斯過程回歸獲得該位置的期望速度：

在實(shí)際應(yīng)用中，利用t時(shí)刻的位置信息可回歸出t時(shí)刻的期望速度，從而可以計(jì)算出t+1時(shí)刻期望的位置：

(3)

在實(shí)際應(yīng)用中，由于該策略的運(yùn)動(dòng)學(xué)實(shí)例均是根據(jù)操作者控制機(jī)械臂運(yùn)動(dòng)的軌跡計(jì)算的，不可避免地帶有操作者的一些操作特征.例如，在目標(biāo)點(diǎn)附近時(shí)，操作者往往會(huì)反復(fù)來回移動(dòng)操作手柄，以保證對(duì)準(zhǔn)目標(biāo)位置.這些操作將會(huì)造成生成的笛卡爾期望軌跡收斂時(shí)間過長(zhǎng)，甚至引起發(fā)散.

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

3 基于動(dòng)力學(xué)約束的機(jī)械臂關(guān)節(jié)控制器設(shè)計(jì)

基于高斯過程的模仿學(xué)習(xí)策略可根據(jù)機(jī)械臂及環(huán)境的狀態(tài)，自主生成對(duì)應(yīng)的笛卡爾任務(wù)分布，具有較強(qiáng)的環(huán)境適應(yīng)性.但是，由于生成的軌跡分布均參考來自操作者的運(yùn)動(dòng)學(xué)實(shí)例，受人類生理特性影響，運(yùn)動(dòng)學(xué)實(shí)例本身就可能不夠平滑，尤其經(jīng)過較長(zhǎng)時(shí)間工作后，操作者肌肉疲勞，這種現(xiàn)象更加明顯；因此，直接將獲得的期望軌跡分布均值用于機(jī)械臂控制，可能會(huì)造成關(guān)節(jié)控制力矩波動(dòng)過大，甚至控制不穩(wěn)定.

針對(duì)上述問題，提出了一種基于動(dòng)力學(xué)約束的機(jī)械臂關(guān)節(jié)控制器.該控制器旨在保證成功完成任務(wù)的同時(shí)，減小關(guān)節(jié)輸出力矩波動(dòng)和能量消耗，使機(jī)械臂的運(yùn)動(dòng)更加平滑.

對(duì)于一項(xiàng)任務(wù)，一般均有多條不同軌跡能滿足要求，即當(dāng)軌跡位于一定的置信區(qū)間內(nèi)，均可認(rèn)為能夠滿足任務(wù)需求.而上節(jié)中的軌跡分布可用于計(jì)算該置信區(qū)間.因此，將控制器設(shè)計(jì)成線性二次跟蹤型(LQT)的形式：

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

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

(4)

式中：∑t為t時(shí)刻期望軌跡分布的協(xié)方差，Q為定值.引入馬氏距離，可以理解為當(dāng)∑t大時(shí)，期望軌跡的不確定性高，機(jī)械臂控制器可以有更大空間調(diào)整，優(yōu)化將傾向于減少輸出力矩，反之亦然.

本文采用微分動(dòng)態(tài)規(guī)劃方法[18]對(duì)式(4)問題進(jìn)行求解.帶偏置的LQT問題的最優(yōu)反饋控制律可寫成如下形式：

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

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

(5)

M(t)和p(t)可通過對(duì)式(5)進(jìn)行數(shù)值積分獲得.參數(shù)Q、R和Qf需要人為調(diào)節(jié)，以使控制器達(dá)到更好的效果.

但是，該控制器工作在機(jī)械臂關(guān)節(jié)空間，而基于高斯過程的模仿學(xué)習(xí)策略的結(jié)果是笛卡爾空間的軌跡分布，因此需要對(duì)其進(jìn)行映射.該分布的均值映射較為簡(jiǎn)單，直接利用逆運(yùn)動(dòng)學(xué)進(jìn)行映射即可.而協(xié)方差映射需要從速度分布映射入手.由于笛卡爾空間的速度和關(guān)節(jié)空間的速度在瞬時(shí)為線性關(guān)系

4 實(shí)驗(yàn)驗(yàn)證

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

表1 D-H參數(shù)

表2 慣量參數(shù)

本文利用空間機(jī)械臂定位螺釘?shù)膶?shí)驗(yàn)驗(yàn)證上述策略，如圖3所示.實(shí)驗(yàn)場(chǎng)所位于天宮二號(hào)空間實(shí)驗(yàn)室內(nèi)，環(huán)境為空間微重力環(huán)境.該實(shí)驗(yàn)是天宮二號(hào)機(jī)械臂利用電動(dòng)工具旋擰螺釘任務(wù)的重要組成部分.

圖2 天宮二號(hào)機(jī)械臂坐標(biāo)系

圖3 電動(dòng)工具定位螺釘實(shí)驗(yàn)

實(shí)驗(yàn)開始時(shí)，空間機(jī)械臂已經(jīng)完成了電動(dòng)工具的抓取，需要通過基于動(dòng)力學(xué)約束的模仿學(xué)習(xí)策略引導(dǎo)機(jī)械臂到達(dá)螺釘上方20 mm處的預(yù)旋擰位置，做好旋擰準(zhǔn)備.具體執(zhí)行步驟如下：

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

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

圖4 運(yùn)動(dòng)學(xué)實(shí)例軌跡

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

由于機(jī)械臂和操作平臺(tái)均為在軌重新組裝，所以地面記錄的位置并不準(zhǔn)確.實(shí)際實(shí)驗(yàn)中發(fā)現(xiàn)，因?yàn)榕擉w膨脹以及操作臺(tái)支架存在縫隙，實(shí)際偏差遠(yuǎn)遠(yuǎn)超出了電動(dòng)工具的容差范圍,但是在利用基于動(dòng)力學(xué)約束的模仿學(xué)習(xí)策略時(shí)仍能完成任務(wù)，體現(xiàn)了該策略具有較好的環(huán)境適應(yīng)性.

Step3:設(shè)計(jì)基于動(dòng)力學(xué)約束的機(jī)械臂關(guān)節(jié)控制器，生成期望的關(guān)節(jié)驅(qū)動(dòng)力矩，并指導(dǎo)空間機(jī)械臂運(yùn)行.初始狀態(tài)下，機(jī)械臂實(shí)測(cè)關(guān)節(jié)角度為[79.85°,-9.44°,-76.48°,-83.47°,-81.54°,-10.46°].將該控制器與計(jì)算力矩控制器對(duì)比，關(guān)節(jié)控制力矩、關(guān)節(jié)角加速度和關(guān)節(jié)速度如圖6～8所示.

由圖6可以看出，與傳統(tǒng)的計(jì)算力矩控制器相比，基于動(dòng)力學(xué)約束的控制器能產(chǎn)生更加平滑的控制力矩.尤其是負(fù)載較大的前3個(gè)關(guān)節(jié)，效果更加明顯.以第1關(guān)節(jié)為例，基于動(dòng)力學(xué)約束的控制器產(chǎn)生的控制力矩的峰-峰值相對(duì)于計(jì)算力矩控制器減小了45%以上，波峰數(shù)也由原來的5個(gè)減小到了3個(gè)，減少了40%，并且有效平滑了尖峰.

由圖7可以看出，應(yīng)用基于動(dòng)力學(xué)約束的控制器可以產(chǎn)生更加平滑的關(guān)節(jié)角加速度.其變化趨勢(shì)與關(guān)節(jié)力矩類似，各個(gè)關(guān)節(jié)角加速度的峰-峰值和波峰數(shù)均有明顯減小，尤其是關(guān)節(jié)6，峰-峰值由1.077 8減小到0.405 2，減小了62%，波峰數(shù)由5個(gè)減小到3個(gè)，減小了40%.應(yīng)用基于動(dòng)力學(xué)約束的控制器時(shí)，關(guān)節(jié)角加速度沒有尖峰，運(yùn)動(dòng)更加平滑.

如由8可以看出，應(yīng)用基于動(dòng)力學(xué)約束的控制器可使機(jī)械臂運(yùn)動(dòng)過程中速度的波峰數(shù)量減少，且波動(dòng)的峰-峰值也會(huì)隨之減小.

能耗是衡量空間機(jī)械臂性能的重要指標(biāo)，本實(shí)驗(yàn)中的能耗對(duì)比如圖9所示.由圖9可知，應(yīng)用基于動(dòng)力學(xué)約束的控制器可使機(jī)械臂運(yùn)行能耗明顯降低，以該任務(wù)為例，相比于應(yīng)用計(jì)算力矩控制器，基于動(dòng)力學(xué)約束的控制器的能耗可減少31%.

圖6 機(jī)械臂關(guān)節(jié)控制力矩

圖7 機(jī)械臂關(guān)節(jié)角加速度

圖8 機(jī)械臂關(guān)節(jié)角速度

圖9 機(jī)械臂能量消耗

圖10為應(yīng)用基于動(dòng)力學(xué)約束控制器時(shí)，機(jī)械臂末端軌跡在置信區(qū)間中的位置.

圖10 實(shí)際笛卡爾軌跡

由圖10可以看出，末端軌跡均位于依據(jù)3σ原則建立的置信區(qū)間內(nèi)，能夠滿足任務(wù)需求.

4 結(jié) 論

1)本文面向空間機(jī)械臂操作任務(wù)，提出了一種基于動(dòng)力學(xué)約束的模仿學(xué)習(xí)策略.該策略主要分為兩個(gè)階段，第一階段為基于高斯過程的模仿學(xué)習(xí)；第二階段為基于動(dòng)力學(xué)約束的機(jī)械臂控制器設(shè)計(jì).與傳統(tǒng)的模仿學(xué)習(xí)加計(jì)算力矩控制器的策略相比，該策略不僅具有操作便捷且克服空間擾動(dòng)的優(yōu)點(diǎn)，還可以有效降低關(guān)節(jié)力矩波動(dòng),同時(shí)減小能量消耗.

2)基于高斯過程的模仿學(xué)習(xí)階段引入改進(jìn)的速度場(chǎng)方法，克服了臨近目標(biāo)位置時(shí)笛卡爾期望軌跡收斂時(shí)間過長(zhǎng)，甚至引起發(fā)散的問題.

3)在基于動(dòng)力學(xué)約束的機(jī)械臂控制器設(shè)計(jì)階段引入馬氏范數(shù)，充分考慮了期望軌跡分布的不確定性，使該控制器的調(diào)整優(yōu)化更符合任務(wù)需求.

4)為驗(yàn)證該方法的有效性，設(shè)計(jì)了機(jī)械臂定位螺釘?shù)膶?shí)驗(yàn).實(shí)驗(yàn)結(jié)果表明，該策略與傳統(tǒng)模仿學(xué)習(xí)加計(jì)算力矩控制的策略相比，大負(fù)載關(guān)節(jié)力矩波動(dòng)的峰-峰值可減少45%，波峰數(shù)可減少40%，能耗可減少31%，且使得關(guān)節(jié)力矩、角加速度以及角速度更加平滑.未來該策略可以嘗試應(yīng)用于接觸任務(wù)和自由漂浮基座空間機(jī)械臂.