基于高斯混合過程的空間機器人任務(wù)空間預(yù)測控制方法

柳子然, 戴梓健, 岳程斐,*, 王培基, 曹喜濱

(1. 哈爾濱工業(yè)大學(xué)航天學(xué)院, 黑龍江 哈爾濱 150001;2. 哈爾濱工業(yè)大學(xué)(深圳)空間科學(xué)與應(yīng)用技術(shù)研究院, 廣東 深圳 518055)

0 引 言

近年來,人類探索宇宙、利用空間資源的腳步不斷加快,提出了一系列長遠計劃,如空間太陽能電站[1]、深空軌道站[2]等,以空間機器人為代表的在軌服務(wù)航天器能夠?qū)崿F(xiàn)在軌制造、裝配等功能,具有廣闊的應(yīng)用前景。但是,傳統(tǒng)方法通常在關(guān)節(jié)空間中獨立地進行規(guī)劃和控制設(shè)計,不能直觀地反映任務(wù)空間中的情況,難以勝任日益復(fù)雜的在軌操控需求。因此,直接在任務(wù)空間中進行控制器設(shè)計能夠緊密圍繞具體任務(wù)開展,日益受到重視[3]。

針對地面機器人的任務(wù)空間控制器設(shè)計已有廣泛研究。文獻[4]首先提出了操作空間控制(operation space control, OCS)方法,利用關(guān)節(jié)空間狀態(tài)與任務(wù)空間狀態(tài)的映射關(guān)系,將常用的關(guān)節(jié)空間機器人動力學(xué)方程轉(zhuǎn)換到任務(wù)空間,再基于反饋線性化得到的線性模型設(shè)計了比例-微分(proportional derivative,PD)控制器并拓展到冗余機械臂中。Hogan等[5]則利用阻抗控制思想,設(shè)計了笛卡爾阻抗控制器,利用阻抗控制思想建立末端接觸力與末端運動間的關(guān)系,并在任務(wù)空間設(shè)計控制器實現(xiàn)期望末端運動。近幾年來,任務(wù)空間控制的研究逐漸擴展到漂浮基機器人中。Lippiello等[6]等推導(dǎo)了帶有一條3自由度(degree of freedom, DOF)機械臂的無人機的動力學(xué)方程,并設(shè)計了笛卡爾阻抗控制器處理接觸力和系統(tǒng)運動間的關(guān)系;Wang等[7]在任務(wù)空間中對雙臂空間機器人的抓捕任務(wù)進行描述,并設(shè)計了滑模控制器以實現(xiàn)基座和末端對期望狀態(tài)的精確跟蹤。雖然以O(shè)CS為代表的任務(wù)空間控制策略不需要在位置層計算逆運動學(xué),但是仍然需要在動力學(xué)轉(zhuǎn)換階段計算雅克比矩陣的逆及導(dǎo)數(shù),在雅克比矩陣不確定或者不滿秩的情況下難以直接應(yīng)用[8]。此外,此類方法缺乏對空間機器人工作中可能遇到的約束的處理能力。

模型預(yù)測控制(model predictive control, MPC)在處理多約束優(yōu)化控制問題上具有顯著優(yōu)勢,因此在機器人、無人機等領(lǐng)域得到了成功的應(yīng)用[9]。徐楊等[10]對無人車的路徑規(guī)劃和跟蹤控制進行統(tǒng)一建模,利用MPC實現(xiàn)無人車輛的避障控制。在空間機器人方面,宗立軍等[11]著重研究了自由漂浮空間機器人避障問題,將關(guān)節(jié)限制、避障要求統(tǒng)一描述為不等式約束,基于MPC提出了混合整數(shù)預(yù)測控制方法。同時,MPC也為解決機器人的任務(wù)空間控制問題提供了思路。針對機器人交互過程中的安全性約束,Nubert等[12]將傳統(tǒng)分離的規(guī)劃層和反饋控制層融合,設(shè)計了MPC器,在任務(wù)空間實現(xiàn)了機械臂末端對設(shè)定點的精確跟蹤;Rybus等[13]設(shè)計了非線性MPC(nonlinear MPC, NMPC),實現(xiàn)了平面自由漂浮空間機器人對任務(wù)空間軌跡的直接跟蹤;Shi等[14]對雙臂空間機器人的任務(wù)空間動力學(xué)模型進行了線性化,并在此基礎(chǔ)上設(shè)計MPC,實現(xiàn)了基座姿態(tài)與機械臂運動的協(xié)同控制。然而,作為一種基于模型的控制算法,MPC的控制精度與模型精度息息相關(guān),同時MPC本身的計算量較大,必須盡量權(quán)衡模型精度和計算復(fù)雜度,以滿足實時性要求。

高斯過程(Gaussian process,GP)回歸作為一種常用的非參數(shù)化貝葉斯回歸方法,需要求解的超參數(shù)較少,因此對樣本數(shù)據(jù)量的需求較小,能夠滿足空間機器人、無人機等非線性系統(tǒng)在工作過程中的快速、精確建模需求,已被引入MPC框架并得到了成功應(yīng)用。Torrente等[15]等利用GP對無人機運動過程中的高速風(fēng)阻進行建模并部署到MPC控制器中,利用實驗證明了GP-MPC的有效性;Woolfrey等[16]利用GP估計了運動基座對機械臂末端位姿的擾動,并利用MPC實現(xiàn)了末端對期望軌跡的跟蹤。但隨著樣本維度和分布范圍的增加,GP的計算復(fù)雜度顯著上升[17]。為此,Leen等[18]提出了高斯混合模型(Gaussian mixture models, GMM),也稱為高斯混合過程,通過對樣本集分組且每組樣本僅對應(yīng)一個GP來降低每個GP的運算量,同時能夠自動適應(yīng)多模態(tài)的樣本集。Liu等[19]利用GMM的思想訓(xùn)練得到了3DOF自由漂浮空間機器人模型;陳友東等[20]通過機械臂抓取實驗演示了GMM相對于GP的優(yōu)勢。

針對空間機器人基座和末端對期望軌跡的直接、精確跟蹤問題,考慮其工作過程中的模型不確定性和物理約束,本文提出了一種基于GMM和MPC(GMM-MPC)的空間機器人任務(wù)空間控制方法。與文獻[13-14]等相比,考慮了基座受控的空間機器人在任務(wù)空間跟蹤過程中受到的物理約束并利用NMPC進行優(yōu)化求解;同時利用高斯混合過程對空間機器人的模型不確定性進行補償和修正,相比于基于單GP(single GP, SGP)的GP-MPC進一步減輕了計算負擔(dān)。

本文主要內(nèi)容安排如下:在第1節(jié)中首先建立了空間機器人標稱模型,并利用GMM進行修正,以應(yīng)對實際工作過程中的模型不確定性;第2節(jié)將建立的空間機器人修正模型應(yīng)用于MPC框架內(nèi),考慮了空間機器人基座的推力分配問題,實現(xiàn)了空間機器人任務(wù)空間的軌跡跟蹤控制;第3節(jié)通過仿真驗證了提出算法的有效性;最后在第4節(jié)對全文內(nèi)容進行了總結(jié)。

1 基于GMM的空間機器人修正模型

1.1 空間機器人建模

本文研究所針對的空間機器人如圖1所示,由單剛體衛(wèi)星基座和n條剛性機械臂構(gòu)成。為了方便描述空間機器人的位置和姿態(tài),建立了以下坐標系:∑I為慣性系;∑B是空間機器人的基座系,原點位于基座質(zhì)心,跟隨基座運動;∑e1～∑en為第1～第n條臂的末端坐標系,以描述空間機器人在任務(wù)空間/笛卡爾空間的運動。此外,針對第m個關(guān)節(jié)定義有關(guān)節(jié)坐標系∑jm,以描述各機械臂在關(guān)節(jié)空間的運動情況。

圖1 空間機器人示意圖

對于配有n條機械臂,每條臂擁有m個自由度,平臺配置L個噴氣推力器的多臂航天器,給出其運動學(xué)和動力學(xué)的簡要表述。

首先,根據(jù)機械臂末端與關(guān)節(jié)、基座狀態(tài)的映射關(guān)系,得到空間機器人末端位姿的描述形式:

(1)

對位姿信息進行求導(dǎo)并整理得到空間機器人末端執(zhí)行器的線速度和角速度、基座的線速度和角速度以及關(guān)節(jié)角速度之間的關(guān)系為

(2)

(3)

式中:JB為基座與各機械臂末端之間的雅克比矩陣;JM為空間機器人各臂與末端之間的雅克比矩陣。

在運動學(xué)模型的基礎(chǔ)上,考慮基座受控情況下,關(guān)節(jié)空間中的機器人標稱動力學(xué)方程:

(4)

式中:HB,HM,HBM分別為空間機器人基座慣量矩陣、機械臂連桿的慣量矩陣、基座與機械臂各連桿耦合慣量矩陣;CB,CM分別為基座和機械臂的非線性項;u0,uq分別為空間機器人基座和機械臂各關(guān)節(jié)作用的控制力和力矩。

整理得到狀態(tài)空間表達式:

(5)

(6)

由此得到基座控制力和力矩與噴氣推力器推力間的關(guān)系:

F0=CIBAT

(7)

式中:CIB為基座本體系到慣性系的轉(zhuǎn)換矩陣;T=[T1,T2,…,TL]T為噴氣推力器推力。

最后,為了便于將動力學(xué)模型應(yīng)用在離散時間控制算法中,設(shè)定采樣時間Δt,將連續(xù)的空間機器人動力學(xué)方程轉(zhuǎn)化為離散時間模型,在當前k時刻測量值的基礎(chǔ)上實現(xiàn)對未來k+N時刻系統(tǒng)狀態(tài)的預(yù)測:

(8)

1.2 基于GMM的擾動估計方法

空間機器人在實際工作過程中,相比于標稱模型,會受到如測量誤差、關(guān)節(jié)摩擦等因素的影響,其實際動力學(xué)模型應(yīng)為

Xk+1=Xk+δt·fnom(Xk,uk)+dk=ftrue(Xk,uk,δt)

(9)

式中:dk表示由于擾動產(chǎn)生的空間機器人干擾力矩。

為此,本文利用GMM對空間機器人所受干擾力矩進行估計補償,如圖2所示。

圖2 基于GMM的擾動訓(xùn)練(左)和估計(右)方法示意圖

GMM在GP的基礎(chǔ)上,為了加快訓(xùn)練和預(yù)測速度,不再使用單一的GP假設(shè),而是采用期望最大化(expectation-maximization, EM)算法對GMM模型進行訓(xùn)練,在訓(xùn)練樣本聚類得到的M個子集基礎(chǔ)上,對每個子集服從的高斯變量進行回歸,最后在預(yù)測時,對各子集對應(yīng)的局部預(yù)測值進行融合得到最終預(yù)測值。

GMM假設(shè)樣本數(shù)據(jù)服從高斯混合分布,即包含A組樣本數(shù)據(jù)的輸入x,其概率密度分布函數(shù)為

(10)

式中:M為樣本子集個數(shù);πi為混合系數(shù),代表樣本來源于第i個高斯分量的概率;p(x|μi,Σi)代表由第i個高斯分量生成樣本的概率;{μi,Σi}為高斯混合過程的超參數(shù),其中μi代表均值向量,Σi代表協(xié)方差矩陣。

與GP的訓(xùn)練類似,GMM的訓(xùn)練過程即超參數(shù){μi,Σi}和混合系數(shù)πi的求解過程。EM算法作為一種超參數(shù)求解的有效算法,核心思想類似最大似然估計或極大后驗概率估計。主要過程分為兩部分:期望步驟(expection-step,簡稱E步驟)和最大化步驟(maximization-step, 簡稱M步驟),兩部分迭代進行。其中,E步驟利用M步驟估計的參數(shù){πi,μi,Σi}計算第a個樣本來自第i個高斯分量的后驗概率ri:

(11)

M步驟通過E步驟計算得出的訓(xùn)練樣本的后驗概率ri,通過極大化對數(shù)似然函數(shù)更新模型參數(shù):

(12)

其次,針對每一組高斯分量進行回歸預(yù)測。擾動估計結(jié)果來自第i個高斯分量的后驗概率為

(13)

(14)

式中:{xi,yi}是第i個高斯分量對應(yīng)的訓(xùn)練數(shù)據(jù);x*是測試數(shù)據(jù);σy是量測噪聲;Ki,K*i,K**i分別為訓(xùn)練數(shù)據(jù)之間、訓(xùn)練數(shù)據(jù)和預(yù)測數(shù)據(jù)之間以及預(yù)測數(shù)據(jù)之間的協(xié)方差,具體計算方式根據(jù)協(xié)方差函數(shù),也稱為核函數(shù)的形式確定。平方指數(shù)核是一種最常見的核函數(shù),由于其連續(xù)可微而便于在NMPC過程中進行雅克比計算[17]:

(15)

式中:σf和l稱為超參數(shù),其求解一般通過最大似然估計實現(xiàn):

(16)

(17)

2 基于MPC的任務(wù)空間控制方法

為了實現(xiàn)空間機器人在任務(wù)空間中的軌跡跟蹤問題,同時應(yīng)對實際工作過程中出現(xiàn)的驅(qū)動器輸入飽和等約束以及模型失配,如圖3所示,本文首先提出了基于GMM的NMPC,然后考慮了操作過程中空間機器人基座噴氣推力器推力分配方法。

圖3 GMM-MPC示意圖

2.1 GMM-MPC設(shè)計

(18)

式中:Qr,Qt分別代表控制誤差和輸入懲罰項的加權(quán)矩陣;Xinit,ξinit分別為優(yōu)化初始時刻采樣得到的空間機器人關(guān)節(jié)空間和任務(wù)空間狀態(tài);umax,umin分別對應(yīng)控制輸入的上下限;N代表預(yù)測步數(shù),通過對時域T進行離散化得到,即N=T/dt。

通過選擇合適的控制器參數(shù),包括Δt,N,Qr,Qt,求解上述優(yōu)化控制問題,得到未來N個時刻的控制量U=[uk,uk+1,…,uk+N-1]T,僅將k時刻控制量uk作為系統(tǒng)輸入。

對于所提的MPC問題,考慮到計算量和實時性的要求,利用CasADi[22]和ACADOS[23]進行求解。其主要思路是首先利用多重打靶將其轉(zhuǎn)化為非線性二次優(yōu)化問題,再利用實時迭代策略下的序列二次規(guī)劃進行求解。

2.2 基座推力分配方法

空間機器人基座運動所需的控制力和力矩由基座配置的噴氣推力器產(chǎn)生。對于GMM-MPC求解得到的控制量uk,需要將其中基座的控制量u0分配并轉(zhuǎn)化成對應(yīng)激活推力器的控制指令。為此,本文構(gòu)造了如下的優(yōu)化問題以解決推力分配問題:

(19)

式中:Tlb,Tub代表推力器的推力飽和約束。

通過求解上述優(yōu)化問題,得到了每個推力器的推力數(shù)值,在提供基座控制需求的同時,滿足能量消耗最少和推力飽和約束。但是,求解得到的是在[Tlb,Tub]間的任意大小的值,而常用的星載冷氣推進器一般只能提供最大推力與零推力兩種模式。因此,為了滿足實際推力器的“開-關(guān)”控制模式,通過脈沖寬度調(diào)制(pulse-width modulation,PWM)方法將連續(xù)推力轉(zhuǎn)換為每個推力器開機時間:

(20)

最后,考慮推力器的最小開機時間Δtc,得到最終的推力器驅(qū)動指令:

(21)

3 仿真校驗

3.1 仿真模型構(gòu)型及參數(shù)

首先,介紹仿真所用的空間機器人系統(tǒng)構(gòu)型及參數(shù)?？紤]到三維多臂機器人系統(tǒng)自由度較高,應(yīng)用所提出算法求解復(fù)雜,同時為了后續(xù)在實驗室已有硬件基礎(chǔ)上進行實驗,本文以一個平面空間機器人模型進行仿真,著重驗證算法的有效性而避免對計算量的討論。其仿真構(gòu)型如圖4所示。

空間機器人模型每個連桿的質(zhì)心位于連桿的中心處;慣性系初始時刻與機器人基座系重合,其慣性參數(shù)如表1所示。由于僅考慮空間機器人的平面運動,因此提供驅(qū)動的8個噴氣推力器沿xB-yB平面布置,即γl=0,?l=1,2,…,8,具體的推力器構(gòu)型參數(shù)見表2,其中[xTl,yTl]T代表推力器安裝位置,βl代表指向角度。

表1 空間機器人慣性參數(shù)

表2 推力器構(gòu)型參數(shù)

3.2 仿真場景設(shè)置

為了驗證本文提出算法的有效性,給定空間機器人初始狀態(tài)ξ0=[0,0,0,0,0,0]T,跟蹤參考軌跡為

(22)

式中:tn=t/tf,tf代表期望運動時間;ξ1,ξf分別為期望軌跡的起始狀態(tài)和末狀態(tài)。

同時,在軌跡跟蹤的過程中,為了讓仿真結(jié)果盡可能地貼合現(xiàn)實情況,參考文獻[17,24]引入兩種形式的模型不確定性:第1種由連桿長度、質(zhì)量、轉(zhuǎn)動慣量等參數(shù)誤差產(chǎn)生,但本文中并非一一對其進行考慮,而是將其集總為中模型參數(shù)的偏差,即將H和C與真值的相對誤差設(shè)置為30%;第2種為周期性擾動:

(23)

3.3 仿真結(jié)果

為了實現(xiàn)第3.2節(jié)中描述的仿真任務(wù)場景,設(shè)計了GMM-MPC控制器進行仿真研究,并將仿真結(jié)果與標準MPC控制器以及SGP結(jié)果進行對比,驗證本文設(shè)計方法的有效性和優(yōu)越性,仿真所用參數(shù)見表3。

表3 仿真參數(shù)

在仿真開始之前,利用標準MPC控制器跟蹤一系列期望軌跡,建立包含1 400組[x,y]的數(shù)據(jù)集,其中1 300組用于訓(xùn)練得到GMM,100組用于進行模型的性能測試。訓(xùn)練和測試結(jié)果如圖5～圖7所示。其中圖5(a)和圖5(b)分別描述了訓(xùn)練得到的高斯混合模型在測試集上的一步預(yù)測結(jié)果,可以看出GMM能夠有效地估計擾動對于空間機器人基座和關(guān)節(jié)的影響,即使對于基座角度和輕質(zhì)關(guān)節(jié)處產(chǎn)生的大幅變動也有較好的估計效果。為了量化并對比GMM估計結(jié)果,計算了均方根誤差(root mean square error, RMSE):

圖5 GMM測試結(jié)果

(24)

為了避免偶然性,計算得到5次預(yù)測的平均RMSE為[0.014 6,0.001 9,0.054 9,0.068 0,0.087 5]。圖6進一步說明了訓(xùn)練樣本數(shù)量與估計結(jié)果,即平均RMSE的關(guān)系,可以看出訓(xùn)練樣本過少時估計效果較差,提供更多的訓(xùn)練樣本可以盡可能地提高精度,但是訓(xùn)練和預(yù)測過程將更加漫長。因此在500～600左右可以實現(xiàn)理想的精度,同時不消耗過多計算資源。最后比較了GMM與SGP的估計結(jié)果,如圖7所示,可以看出相同樣本數(shù)量情況下GMM比SGP精度有著明顯改善。隨著樣本數(shù)量的提升,SGP精度提升明顯。

圖6 GMM估計結(jié)果與訓(xùn)練樣本間的關(guān)系

圖7 GMM與SGP結(jié)果對比

最后,針對空間機器人的期望軌跡跟蹤問題,分別設(shè)計了基于標準MPC和部署了訓(xùn)練得到的GMM的GMM-MPC進行實時仿真,以驗證提出方法的有效性,仿真結(jié)果如圖8所示。對于沒有進行不確定性補償?shù)臉藴蔒PC控制器,其預(yù)測使用的標稱動力學(xué)模型與實際動力學(xué)模型存在明顯差異,導(dǎo)致預(yù)測時域內(nèi)的系統(tǒng)狀態(tài)估計值與實際值不符,進而影響控制信號求解。由于MPC本身具有一定的魯棒性,因此雖然圖11中所示的標準MPC跟蹤誤差仍存在收斂趨勢,但在30 s時依然出現(xiàn)震蕩,收斂時間長,控制性能較差。

圖8 空間機器人期望軌跡

而GMM作為一種有效的數(shù)據(jù)驅(qū)動非線性系統(tǒng)建模方法,能夠?qū)δＰ筒淮_定性進行較為精確的補償。由圖9和圖10可以看出,基于GMM-MPC的空間機器人控制器在5 s以內(nèi)控制誤差即收斂到0,與如圖11所示的標準MPC跟蹤情況相比,有了明顯的改善,特別是針對末端狀態(tài)的控制,由于擾動累積和基座/機械臂質(zhì)量比的影響,末端處模型不確定性較大,在標準MPC控制器作用下,相較于基座控制性能更差,但在GMM-MPC作用下其性能得到了有效提升。

圖9 GMM-MPC跟蹤結(jié)果

圖10 GMM-MPC跟蹤誤差

圖11 標準MPC跟蹤誤差

但同時可以注意到,圖9、圖10所示的跟蹤結(jié)果相比期望軌跡仍然存在一些偏差,這一方面是由于噴氣推力器存在死區(qū)特性,更主要的原因在于GP,包括GMM,其長期預(yù)測能力有限,因此在測試集上估計效果較好,但是在仿真應(yīng)用中,隨著MPC預(yù)測時域的增長,GP的估計精度會有所下滑[19]。

整個任務(wù)過程中的推力器激活情況如圖12、圖13所示,初始階段由于軌跡跟蹤誤差較大,部分推力器激活時間較長。由于需要抑制模型不確定性和機械臂運動的影響,以及推力器存在死區(qū)特性,隨著跟蹤誤差逐漸收斂,推力器激活時間減短,但不會完全關(guān)閉。

圖12 推力器T1-T4啟動情況

圖13 推力器T5-T8啟動情況

4 結(jié) 論

本文針對空間機器人的任務(wù)空間軌跡跟蹤控制問題,提出了一種基于高斯混合過程的MPC方法。首先,利用MPC解決了空間機器人基座和末端位姿對設(shè)定點的直接跟蹤,同時滿足輸入飽和等實際物理約束。為了解決預(yù)測過程中由于模型不確定性帶來的干擾,利用預(yù)實驗數(shù)據(jù)訓(xùn)練得到高斯混合過程以估計標稱動力學(xué)的誤差,并部署到MPC框架中。最后,考慮到空間機器人基座的驅(qū)動形式,設(shè)計了噴氣推力器的推力分配方法。仿真結(jié)果表明,相比于SGP,GMM需要樣本更少、預(yù)測效果更好,本文提出的基于高斯混合過程的MPC方法能夠有效地實現(xiàn)空間機器人對任務(wù)空間參考軌跡的跟蹤,保證任務(wù)成功完成的同時更加直觀、簡便。