劉恒力，劉清宇，郭永興

(武漢科技大學 a.機器人與智能系統(tǒng)研究院；b.冶金裝備及其控制教育部重點實驗室；c.機械傳動與制造工程湖北省重點實驗室，武漢 430081)

0 引言

足式機器人由于具有良好的地形適應性和高度的靈活性，可以在輪式或履帶式車輛無法到達的困難地形環(huán)境上行駛。隨著足式機器人在這樣的地形上運動，其與外部環(huán)境的接觸檢測顯得尤為重要。目前，大多數(shù)足式機器人采用外部傳感器檢測接觸碰撞和無外部傳感器的接觸碰撞檢測算法。采用外部傳感器檢測接觸碰撞[1-2]，文獻[3]在機器人足端安裝力傳感器，根據(jù)反饋的信息對機器人運動節(jié)律和關節(jié)信息進行實時調節(jié)。然而采用外部傳感器檢測接觸碰撞會增加腿部的轉動慣量，額外的傳感器也會增加成本和系統(tǒng)的復雜性，且安裝在足端的傳感器在受到沖擊力時容易損壞，導致機器人的可靠性降低。另一種是不采用外部傳感器而是通過本體傳感器檢測接觸碰撞[4-6]。該方法通常利用足端的力的估計值來實現(xiàn)對觸地的檢測。文獻[7]提出了一種本體感知驅動模式，通過低傳動比的驅動、關節(jié)角度和來自慣性測量單元(IMU)的反饋來尋求接觸力，利用關節(jié)處的力控制，有效地控制足部的接觸作用力。文獻[8]基于浮動基動力學模型，提出了一種新的殘差公式估計外部接觸力。文獻[9]開發(fā)了一種堅固的腿測距模塊，它使用內部力感測來檢測腳的撞擊，但是忽略了加速度測量值的誤差影響。文獻[10]提出了一種在融合多個先驗的概率接觸檢測的統(tǒng)一框架內追求基于廣義動量的擾動觀測器,用來檢測和處理意外的過早和過晚的接觸碰撞。

隨著機器學習理論和技術的成熟，從數(shù)據(jù)中學習模型已日益成為機器人建模、規(guī)劃及控制的重要方法。機器人逆動力學滿足關節(jié)空間到力矩空間的唯一映射關系，逆動力學的學習屬于標準的回歸問題。從觀測數(shù)據(jù)中對隱含的函數(shù)關系進行擬合，如高斯過程回歸[11-13]、支持向量機回歸[14]等方法。高斯過程(Gaussian processes，GP)作為貝葉斯機器學習方法之一，以概率分布來表示先驗知識，在此基礎上構建預測模型。文獻[15]采用高斯過程回歸方法，通過 SCARA 機器人的輸入與輸出量對其逆向動力學模型進行估計，并用于計算力矩控制中，得到了不錯的仿真結果。文獻[16]基于穩(wěn)定模型的高斯過程回歸機器人控制，提出了一種新的計算轉矩控制律，其跟蹤精度方面優(yōu)于傳統(tǒng)的計算轉矩方法。

本文提出采用一種免外部傳感器的基于高斯過程回歸的機器人接觸碰撞力檢測方法，首先，給出機器人的足端力模型，確立關節(jié)驅動轉矩與足端力之間的關系。然后，在Simmechanics里建立雙關節(jié)單腿機器人的物理模型，給定關節(jié)空間運動的角度和角速度為輸入量，以關節(jié)所需的實際驅動力矩為輸出量，利用訓練數(shù)據(jù)通過高斯過程回歸進行系統(tǒng)的逆動力學模型的擬合，用預測數(shù)據(jù)驗證得到的逆動力學模型的準確性。最后，將得到的逆動力學模型用于足端力的檢測。

1 基于高斯過程回歸的碰撞檢測建模

1.1 足端力建模

為研究單腿機器人的受力情況，本文將單腿機器人簡化為二連桿結構。采用拉格朗日方程建立單腿機器人的動力學模型如下：

(1)

基于動力學模型的方法檢測接觸碰撞是通過計算估計轉矩與實際轉矩之間的差值：

τk=τ′-τ

(2)

式中，τk為足端力引起的各關節(jié)轉矩，τ′為有足端力時各關節(jié)的實際輸入轉矩，τ為無足端力時各關節(jié)的計算輸入轉矩。關節(jié)轉矩和足端力之間的關系滿足：

τk=JT×Fk

(3)

式中,Fk為機器人足端力；J為機器人的速度雅克比矩陣；τk為足端力引起的關節(jié)轉矩。

由式(1)～式(3)可推導出足端力：

(4)

從上式可知，求解足端力需要精確的動力學模型參數(shù)，然而動力學模型參數(shù)辨識推導復雜,受摩擦力和各關節(jié)的加速度誤差的影響，辨識精度不高，導致力矩殘差中含有誤差：

τk+δτ=τ′-τ

(5)

δτ包括模型誤差和測量誤差。減小此誤差將提高足端力檢測的精度，本文采用基于高斯回歸過程建模來學習動力學參數(shù)。

1.2 高斯過程回歸建模原理

高斯過程回歸方法是貝葉斯優(yōu)化方法的一種，在處理小樣本、維數(shù)高和非線性等回歸問題上具有很好的效果。給定輸入X和輸出Y， 回歸的任務就是學習輸入和輸出之間的映射關系，利用這個映射關系，預測新輸入x*對應的輸出量y*?？梢远x出一個高斯過程來描述函數(shù)的分布，高斯過程的特征由其均值函數(shù)m(x)和協(xié)方差函數(shù)k(x,x′)來確定：

m(x)=E[f(x)]

(6)

k(x,x′)=E[(f(x)-m(x))(f(x′)-m(x′))]

(7)

GP可以定義為：

f(x)～GP[m(x),k(x,x′)]

(8)

(9)

假設訓練集為(x,y)，測試集為(x*,y*)，可以得到觀測值y和預測值f*的聯(lián)合先驗分布為：

(10)

其中，K(X*,X*)為測試點x*自身協(xié)方差矩陣，K(X,X)為訓練點的協(xié)方差矩陣，K(X,X*)=K(X*,X)是測試點x*和訓練集點x之間的協(xié)方差矩陣，由此可以計算出預測值f*后驗分布為：

f*|X,y,x*～N(μ,Σ)

(11)

其中，

μ=K(x*,X)[K(X,X)+σ2I]-1y

(12)

Σ=K(X,x*)-K(x*,X)[K(X,X)+σ2I]-1K(X,x*)

(13)

其中，μ和Σ為測試點x*對應的預測值f*的均值和協(xié)方差。

由上述公式可以看出，高斯過程回歸算法,對于預測輸出值,并非單純估算一個預估值,還會估算出預估值方差。這種方式將預測的不確定性考慮進去,更貼近實際。

選取協(xié)方差函數(shù)來對高斯過程進行訓練，如平方指數(shù)協(xié)方差函數(shù)：

(14)

(15)

獲得超參數(shù)后，利用式(12)、式(13)就可以得到測試點x*對應的預測值f*的均值和方差。

1.3 利用高斯過程進行足端碰撞力檢測

(1)首先選擇預測模型的輸入變量和輸出變量，輸出變量為各關節(jié)的驅動轉矩，影響各關節(jié)的驅動轉矩的因素有各關節(jié)的角度、角速度、角加速度以及摩擦轉矩，而摩擦轉矩又受角速度影響，因此它對驅動轉矩的影響可以歸結到角速度上，由于高斯回歸過程只是輸入量與輸出量之間的函數(shù)映射關系，而與具體的物理系統(tǒng)無關，這里采用前一時刻，后一時刻的角速度代替角加速度對驅動轉矩的影響以減少角加速度的誤差對系統(tǒng)模型造成的影響。因此，選擇各關節(jié)角度、角速度、前一時刻角速度，后一時刻角速度作為輸入變量，選擇各關節(jié)驅動轉矩為輸出變量。

(2)利用Simulink進行建模仿真，得到無足端力和有足端力的兩份數(shù)據(jù)集，再將無足端力數(shù)據(jù)集分為兩部分，一部分被選為訓練集作為建模數(shù)據(jù)集，另一部分被選為測試集，用來檢測模型的準確性。

(3)選擇先驗分布，定義似然函數(shù)。

(4)選擇協(xié)方差函數(shù)，根據(jù)訓練樣本集通過共軛梯度法迭代求解得出最優(yōu)超參數(shù)。

圖2 Simulink單腿機器人控制系統(tǒng)

(5)輸入測試集的輸入，根據(jù)所得的動力學模型預測輸出，與預測集的真實輸出比較，檢查精度是否符合要求，若符合要求，則結束系統(tǒng)的逆動力學模型的建立，否則，重復第(3)、(4)步。

(6)將得到的系統(tǒng)概率動力學模型用于預測有足端力時的驅動轉矩，依據(jù)前面的足端力模型，檢測足端力的大小。

2 單腿機器人足端碰撞仿真實驗

2.1 Simulink模型建立和配置

為了驗證模型的有效性，本節(jié)搭建了單腿機器人的物理模型，設計了仿真實驗。首先，基于Simulink的Simmechanics庫建立了單腿機器人的物理模型，如圖1所示。

圖1 單腿機器人模型

該單腿機器人有兩個轉動關節(jié)，分別為髖關節(jié)和膝關節(jié)。機器人身體長0.6 m，寬0.3 m，高0.15 m，質量為12 kg，大腿長0.6 m，腿截面寬度為0.06 m，質量為1 kg，小腿長0.6 m，腿截面寬為0.06 m，質量為1 kg。

2.2 學習過程

首先固定身體，以身體質心為坐標原點，建立參考坐標系，對機器人足端進行軌跡規(guī)劃，對水平方向的起點、終點的位置，速度和加速度進行約束，進行五次多項式擬合得到足端的運動軌跡：

x=230.4×t5-288×t4+96×t3-0.6

(16)

選擇足端運動軌跡為拋物線，豎直方向軌跡規(guī)劃為：

(17)

通過逆運動學得到其關節(jié)空間的規(guī)劃角度，采用逆動力學模型前饋加PD反饋控制得到關節(jié)的驅動力矩(此處的逆動力學模型的參數(shù)僅為一個大致的參數(shù))。摩擦力矩采用庫倫-粘滯模型定義，將關節(jié)驅動轉矩輸入給單腿機器人的物理模型，使單腿機器人按照規(guī)劃的軌跡運動。

Simulink系統(tǒng)框圖如圖2所示，設置仿真時間為0.5 s，步長為0.000 5 s，仿真得到膝關節(jié)和髖關節(jié)的關節(jié)角度、關節(jié)角速度，前一時刻的關節(jié)角速度，后一時刻的關節(jié)角速度，關節(jié)驅動力矩的數(shù)據(jù)，由于實際數(shù)據(jù)可能還有噪聲，因此給關節(jié)驅動力矩加入了高斯噪聲。將數(shù)據(jù)分為兩組，由于涉及到前一時刻的關節(jié)角速度和后一時刻的關節(jié)角速度，取訓練數(shù)據(jù)167組，預測數(shù)據(jù)166組，利用訓練數(shù)據(jù)進行系統(tǒng)逆動力學模型的學習，得到腿部的系統(tǒng)逆動力學模型，用預測數(shù)據(jù)檢測學習到的模型。檢查精度是否符合要求，若不符合，則重新學習。

得到系統(tǒng)的逆動力學模型后，給定一個足端力，按照規(guī)劃的軌跡再一次進行仿真。根據(jù)得到的模型預測所需的髖關節(jié)和膝關節(jié)的驅動力矩，即為無足端力時所需的驅動力矩，與有足端力時實際所需的驅動力矩之差，即為足端力碰撞力矩。根據(jù)足端力模型可以求解足端力大小。通過改變規(guī)劃的足端軌跡的步長和步高，可以再一次進行仿真實驗以進行系統(tǒng)動力學模型的學習。

不固定身體，讓其從空中掉落，最后觸地豎直跳躍。以地面坐標系作為參考坐標系，空中相時保持機器人的髖關節(jié)和膝關節(jié)的關節(jié)角度固定不變，髖關節(jié)角度為0.698 rad(相對于負z軸逆時針的旋轉角度)，膝關節(jié)角度為-1.396 rad(相對于大腿順時針旋轉的角度)，當進入觸地相時，假設觸地瞬間足端速度迅速變?yōu)?，足端與地面保持固定不動，利用基于SLIP模型的觸地相模型對機器人髖關節(jié)質心作軌跡規(guī)劃，通過逆運動學得到其髖關節(jié)和膝關節(jié)空間規(guī)劃角度，再對其進行四次多項式擬合得到:

qh=108.715*t4-66.66*t3-2.695*t2+3.959*t+0.698

(18)

qk=-217.429*t4+133.32*t3+5.390*t2-7.918*t-1.396

(19)

空中相時采用PID反饋控制，觸地相時采用PD反饋控制關節(jié)的驅動力矩，機器人從足端距地面0.48 m處落下，0.313 s觸地，觸地相時間為0.306 6 s，設置仿真時間為0.933 s，步長為0.000 2 s，重復身體固定時的過程對其進行學習，取訓練數(shù)據(jù)200組，預測數(shù)據(jù)120組，得到整個機器人系統(tǒng)的逆動力學模型。

2.3 仿真實驗結果

身體固定時，關節(jié)驅動轉矩預測值與真實值及其殘差如圖3、圖4所示。

圖3 身體固定的髖關節(jié)驅動轉矩預測值與真實值及其殘差

圖4 身體固定的膝關節(jié)驅動轉矩預測值與真實值及其殘差

可以看出，髖關節(jié)轉矩殘差最大為0.985 N·m，發(fā)生在0.306 s，此時由于髖關節(jié)角速度方向改變而導致髖關節(jié)摩擦轉矩突變，而導致此處附近轉矩殘差較大，其他位置的殘差最大為0.279 N·m。膝關節(jié)轉矩殘差最大為0.979 N·m，發(fā)生在0.252 s，此時由于膝關節(jié)角速度方向改變而導致膝關節(jié)摩擦轉矩突變，而導致此處附近殘差較大，其他位置的殘差最大為0.213 N·m。殘差為模型誤差和測量誤差，此處受摩擦轉矩影響較大。

當在0.22 s～0.25 s內加入一個fx=9 N，fz=9 N的足端力時，用得到的系統(tǒng)逆動力學模型進行預測，關節(jié)驅動轉矩預測值與真實值如圖5所示。

圖5 有足端力時關節(jié)驅動轉矩預測值與真實值

圖中曲線表明，當機器人發(fā)生碰撞時，利用學習得到的系統(tǒng)逆動力學模型獲取的驅動轉矩預測值與驅動轉矩的真實值之間存在一個較為明顯的差值，這個差值就是由于足端發(fā)生碰撞而引起，其大小就是驅動關節(jié)碰撞力矩τk，如圖6所示。

圖6 碰撞力矩

從圖6可以明顯看出，在0.22 s～0.25 s之間，髖關節(jié)碰撞碰撞力矩迅速增加至3.6 N·m左右，膝關節(jié)碰撞力矩迅速增加至4.9 Nm左右，在之后還有一段波動值。根據(jù)得到的關節(jié)碰撞力矩，利用足端力模型可以得到檢測的足端碰撞力大小如圖7所示。

圖7 碰撞力

當身體不固定時，讓其從空中掉落，最后觸地豎直跳躍。其動力學模型與身體固定時有所差異，因為相比于機器人身體固定，其動能和勢能有所改變，拉格朗日動力學方程的系數(shù)有所改變。因此需要對機器人整體的動力學模型參數(shù)進行預測，機器人在0.313 s時觸地，0.620 s時起跳。

由于機器人觸地瞬間會有一個較大的沖擊力，因此角速度會在一瞬間發(fā)生較大的變化，而高斯回歸訓練時對輸入變量相差較大時預測結果精度較低，因此訓練集的角速度參數(shù)也要設計成迅速變化，以此來提高高斯回歸過程預測的精度。關節(jié)驅動轉矩預測值與真實值及其殘差如圖8、圖9所示。

圖8 身體運動的髖關節(jié)驅動轉矩預測值與真實值及其殘差

圖9 身體運動的膝關節(jié)驅動轉矩預測值與真實值及其殘差

經過訓練得到系統(tǒng)的逆動力學模型后，用其來檢測足端碰撞力的大小，其關節(jié)驅動轉矩預測值與真實值如圖10所示，足端碰撞引起的關節(jié)碰撞力矩如圖11所示。

圖10 觸地碰撞時關節(jié)驅動轉矩預測值與真實值

圖11 觸地碰撞時的碰撞力矩

從圖中可以看出，當機器人在0.313 s觸地時，由于受到地面的沖擊力作用，其關節(jié)角加速度，關節(jié)角速度發(fā)生突變，導致所需的驅動轉矩迅速增大，隨后，沖擊力迅速變小，地面對機器人的支撐力逐漸增加，真實的驅動轉矩隨著觸地力的變化而發(fā)生相應的變化。碰撞力矩是真實的驅動轉矩與預測的驅動轉矩的差值，其變化趨勢反映的是碰撞力的變化趨勢。

實際足端碰撞力與通過系統(tǒng)的逆動力學模型計算出的檢測力如圖12、圖13所示。

圖12 水平方向檢測碰撞力與實際碰撞力 圖13 豎直方向檢測碰撞力與實際碰撞力

3 分析與討論

當身體固定時，從圖5中可以看出足端碰撞引起的關節(jié)力矩在0.25 s～0.35 s之間波動較大，與實際出入較大，分析可知這是由于關節(jié)角速度方向改變導致摩擦轉矩突變引起，由于訓練數(shù)據(jù)中角速度方向改變的數(shù)據(jù)僅有兩個，所以模型預測不夠精確。增加訓練數(shù)據(jù)中關于關節(jié)轉速方向轉變的個數(shù)，再次訓練得到模型進行預測。利用修改后的逆動力學模型得到足端碰撞引起的關節(jié)力矩的檢測值如圖14所示。足端碰撞力檢測值如圖15所示。

圖14 模型修改后的碰撞力矩

圖15 模型修改后的碰撞力

修改后的系統(tǒng)逆動力學模型對足端力的檢測消除了摩擦轉矩突變帶來的誤差，更加精確。

當機器人身體不固定時，著地瞬間由于存在慣性及動能，關節(jié)速度瞬時出現(xiàn)尖峰值，關節(jié)角度瞬時出現(xiàn)較大的變化值。觸地瞬間足端受到一個較大的沖擊力，又迅速減小，在機器人腿部收縮過程中足端受力又逐漸增加，直到機器人腿部收縮到最底部時又達到一個極值，然后隨著機器人腿部的伸展又逐漸減小，直至機器人足端離地，受力減小為0。從圖11和圖12可以看出，足端實際水平方向最大受力為732 N，在最底部實際水平方向最大受力為271 N，檢測到的水平方向最大受力為683 N，誤差為6.69%，在最底部檢測到的水平方向最大受力為240 N，誤差為11.4%。足端實際豎直方向最大受力為1046 N，在最底部實際豎直方向最大受力為683 N，檢測到的豎直方向最大受力為952 N，誤差為8.99%，在最底部檢測到的豎直方向最大受力為611 N，誤差為10.5%。由于觸地瞬間角速度突變，摩擦轉矩突變，轉矩輸出存在滯后等因素的影響，使檢測到的足端碰撞力比實際碰撞力小，且存在一定的滯后現(xiàn)象。

4 結論

針對單腿機器人足端觸地檢測問題，提出了一種基于高斯過程回歸建模獲得系統(tǒng)的逆動力學模型的方法。通過仿真得到輸入量和輸出量，對模型加以訓練，建立關節(jié)驅動力矩與關節(jié)角度和關節(jié)角速度之間的映射函數(shù)。根據(jù)建立的映射關系，可以得到機器人各關節(jié)的驅動力矩，當機器人足端發(fā)生碰撞時，通過驅動關節(jié)的實際力矩和預測力矩，可以檢測足端碰撞力的大小。

仿真結果表明，本文提出的方法能夠實現(xiàn)機器人足端觸地的檢測，避免了測量誤差以及對摩擦力、驅動器進行建模以及復雜繁瑣的動力學參數(shù)辨識的問題，大大減小了動力學模型與實際模型之間的誤差。但是本文考慮的噪聲為高斯噪聲，沒有考慮其他的噪聲干擾，對其他噪聲干擾的數(shù)據(jù)，預測的模型可能不夠準確。同時，由于在計算過程中涉及到矩陣的求逆運算，計算量相對較大。

后期的工作中，將采用組合協(xié)方差函數(shù)探索研究各種噪聲下的建模預測以提高預測精度，并對減少計算量，實現(xiàn)更快速的檢測進行更進一步的研究。