馬 斌

(重慶工商大學(xué)機(jī)械工程學(xué)院，重慶 400067)

0 引言

機(jī)器人不僅在工業(yè)領(lǐng)域得到了廣泛的認(rèn)可，而且在人類生活領(lǐng)域也發(fā)揮著重要作用。人形機(jī)器人的形狀類似人類，因此它們對(duì)人類較為友好，可以使用為人類設(shè)計(jì)的系統(tǒng)。這是人形機(jī)器人被期望積極參與人類生活空間的原因之一[1]。為了在這樣的生活空間中成功地工作，人形機(jī)器人必須能夠表現(xiàn)得與人類相似。由于這個(gè)原因，人形機(jī)器人的行走和奔跑運(yùn)動(dòng)已經(jīng)得到了深入的研究[2]。攀爬運(yùn)動(dòng)的控制方法也可以潛在地應(yīng)用于崎嶇地形環(huán)境下的運(yùn)動(dòng)。

攀爬墻體是通過在肢體和地形特征之間產(chǎn)生接觸力來完成的，例如突出物、洞和裂縫，這些統(tǒng)稱為“支撐物”。以往的方法需要兩個(gè)步驟才能完成爬升運(yùn)動(dòng)[3]。在第一步中，確定仿人機(jī)器人的可能路徑。在第二步中，確定肢體跟蹤路徑所需的運(yùn)動(dòng)。Almonacid M等提出的路徑規(guī)劃方法[4]，首先探索每個(gè)肢體適用的支撐物組合，然后尋找從開始到目標(biāo)的一系列可行的支撐物組合。接著在考慮機(jī)械約束的同時(shí)，生成肢體軌跡以遵循上一步中找到的路徑。換言之，傳統(tǒng)的控制方法一步一步地分別產(chǎn)生路徑和運(yùn)動(dòng)。因此，不能實(shí)現(xiàn)攀爬運(yùn)動(dòng)的整體優(yōu)化以最大化攀爬效率。

為解決這一問題，本文提出了一種控制方法，能夠同時(shí)優(yōu)化攀爬路徑和運(yùn)動(dòng)，使攀爬運(yùn)動(dòng)變得更加流暢、實(shí)時(shí)性更高。將仿人機(jī)器人的攀爬任務(wù)視為一個(gè)考慮機(jī)械約束的NMPC問題[5]，并使用基于墻體圖的狀態(tài)相關(guān)權(quán)重和勢函數(shù)。在現(xiàn)有方法中，當(dāng)墻體的信息發(fā)生變化(例如檢測到新的支撐物)時(shí)，機(jī)器人必須停止爬升并生成新的路徑[6]。相比之下，在提出的方法中，如果計(jì)算速度足夠高，則可以實(shí)時(shí)處理墻體信息的變化。但是，使用具有多個(gè)支撐的復(fù)雜墻體模型來計(jì)算控制輸入需要花費(fèi)大量的時(shí)間。因此，為了減少計(jì)算成本，還提出了一種視距評(píng)估方法，以便從所有可用的信息中選擇必要的信息。

1 系統(tǒng)建模

1.1 仿人機(jī)器人模型

圖1 十自由度連桿 仿人機(jī)器人模型

研究采用了一個(gè)在垂直平面上具有10個(gè)自由度和10個(gè)連桿的機(jī)器人模型，如圖1所示。假設(shè)機(jī)器人僅在垂直平面上移動(dòng)得足夠慢。因此，只考慮運(yùn)動(dòng)學(xué)模型而不考慮動(dòng)力學(xué)模型。每個(gè)連桿的端坐標(biāo)用Qi=(Qi,x，Qi,y)表示。并且機(jī)器人的右手、左手、右腳和左腳的端點(diǎn)坐標(biāo)分別表示為q1、q2、q3和q4=(Q3、Q5、Q8和Q10)。正方向定義為逆時(shí)針方向[7]。相對(duì)角度矢量φ定義為：

(1)

式中，φi為與根連桿的相對(duì)角度。但是，φ1和φ2是從水平軸到Q0的相對(duì)角度。將連桿i的坐標(biāo)系到絕對(duì)坐標(biāo)系的齊次變換矩陣定義為Ti(φ,Q0)。

(2)

式中，Q0為連接桿件1和6的關(guān)節(jié)坐標(biāo)，且被定義為機(jī)器人的中心；Qi為每個(gè)連桿的端點(diǎn)坐標(biāo)；φ為相對(duì)姿態(tài)角；li為連桿i的長度；mi為連桿i的重量。機(jī)器人中心和每個(gè)肢體末端的垂直速度表示為：

(3)

機(jī)器人在保持平衡的同時(shí)，通過每個(gè)關(guān)節(jié)和手動(dòng)致動(dòng)器在支撐物上產(chǎn)生支撐力和扭矩來攀爬力和扭矩的平衡方程[8]表示如下:

(4)

(5)

(6)

式中，g為重力加速度；Qg,i為每個(gè)連桿的質(zhì)心坐標(biāo)，。fi,x為每個(gè)肢體qi(i=1,...,4)端點(diǎn)在水平方向上的支撐力；fi,y為每個(gè)肢體qi(i=1,...,4)端點(diǎn)在垂直方向上的支撐力；τi為每個(gè)肢體qi(i=1,...,4)的端點(diǎn)產(chǎn)生的扭矩。

1.2 墻體模型

假設(shè)墻體是垂直平面，并將其設(shè)置為xy平面。y軸正方向被視為垂直正方向。假設(shè)當(dāng)每條肢體的端點(diǎn)處于保持狀態(tài)(支撐物上)時(shí)，機(jī)器人可以產(chǎn)生夾持力。墻體上每個(gè)點(diǎn)(x,y)在y軸方向上可以產(chǎn)生的最大夾持力表示為：

(7)

式中，fpi,y為在支撐i處沿y軸方向可以產(chǎn)生的最大夾持力，其坐標(biāo)點(diǎn)是(xi,yi)。我們稱這個(gè)函數(shù)為最大夾持力函數(shù)。圖2給出了該函數(shù)的一個(gè)示例。類似地，在支撐i處x軸方向的最大夾持力和最大扭矩分別由fpi,x和τpi表示。還考慮了最大持力函數(shù)Fmax,x(x,y)和Tmax(x,y)。

圖2 最大夾持力函數(shù)

2 提出的控制方法

2.1 非線性模型預(yù)測控制

本文采用實(shí)時(shí)最優(yōu)控制中的NMPC來處理非線性的、有約束的控制問題。Kayode owa等[5]對(duì)NMPC的多種應(yīng)用案例有詳細(xì)的解釋說明。Yu M等[9]已經(jīng)考慮了將攀爬任務(wù)定義為NMPC的基本思想，但是其設(shè)置非常簡單。而在本文將對(duì)基于NMPC的仿人攀爬機(jī)器人運(yùn)動(dòng)進(jìn)行更加詳細(xì)的分析。

在NMPC中，非線性系統(tǒng)的狀態(tài)描述方程為：

(8)

式中，x(t)為狀態(tài)向量，u(t)為輸入向量，p(t)為給定時(shí)變參數(shù)?？紤]從當(dāng)前時(shí)間t到有限未來時(shí)間t+T的性能指標(biāo)J。NMPC中最優(yōu)控制輸入為uopt(t)(t≤τ≤t+T)，同時(shí)考慮如下約束條件：

C(x(τ),u(τ),p(τ))=0

(9)

可見最優(yōu)控制輸入uopt(τ)取決于當(dāng)前時(shí)間t和狀態(tài)x(t)，因此，它可以表示為uopt(τ;t,x(t))。將最優(yōu)控制輸入的初始值用作時(shí)間t處的實(shí)際控制輸入。

u(t)=uopt(t;t,x(t))

(10)

這樣，在每個(gè)采樣時(shí)間，通過求解直到有限未來的最優(yōu)控制問題來計(jì)算控制輸入。采用Continuation/GMRES算法作為實(shí)時(shí)數(shù)值優(yōu)化方法實(shí)現(xiàn)求解線性方程組。對(duì)于NMPC來說，只要計(jì)算成本不是太高，任何求解大規(guī)模非對(duì)稱稀疏線性方程組的最常用方法都可以應(yīng)用到該問題中。

2.2 控制模型

為了讓人形機(jī)器人使用NMPC實(shí)現(xiàn)攀爬運(yùn)動(dòng)，設(shè)狀態(tài)x為：

(11)

且設(shè)輸入u為：

(12)

式中，f為夾持力；d為偽輸入集合，由偽輸入di(i=1,...,12)組成。

(13)

然后，狀態(tài)方程表示為：

(14)

由每個(gè)肢體qi端點(diǎn)產(chǎn)生的夾持力fi,x、fi,y和扭矩τi的約束條件為：

-fmaxi,x≤fi,x≤fmaxi,x

(15)

-fmaxi,y≤fi,y≤fmaxi,y

(16)

-τmaxi≤τi≤τmaxi

(17)

其中，

fmaxi,x=Fmax,x(qi)

(18)

fmaxi,y=Fmax,y(qi)

(19)

τmaxi=Tmax(qi)

(20)

可以使用偽輸入di將上述方程轉(zhuǎn)換為如下情況：

(21)

(22)

(23)

上述3個(gè)方程和方程(4)～方程(6)是NMPC中的約束條件。

2.3 路徑和運(yùn)動(dòng)的綜合優(yōu)化方法

(1)性能指標(biāo)：攀爬性能指標(biāo)J表示如下：

(24)

(25)

式中，Sm,Sh,Sp,Su,Sd,Smf為權(quán)重；W為勢函數(shù)；pi為變權(quán)重；φref為參考姿態(tài)角。L(x(τ),u(τ))和η(x(t+T))的第一項(xiàng)通過勢函數(shù)W引導(dǎo)機(jī)器人的肢體攀爬，這將在本節(jié)后面解釋。L(x(τ),u(τ))L(x(τ),u(τ))的第二項(xiàng)中變權(quán)重pi定義為:

(26)

式中，vref為目標(biāo)爬升速度。pi代表了每個(gè)肢體的實(shí)際位置和目標(biāo)位置之間的差異。例如，如果肢體長時(shí)間停留在一個(gè)保持狀態(tài)，pi值會(huì)增加，因此該肢體會(huì)被引導(dǎo)到y(tǒng)軸正方向。L中第一項(xiàng)和第二項(xiàng)之間的大小變化，會(huì)導(dǎo)致在夾住一個(gè)支撐和釋放另一個(gè)支撐之間的切換，并提示應(yīng)該選擇哪些肢體釋放它們的支撐。L的第三項(xiàng)穩(wěn)定機(jī)器人的姿勢，第四項(xiàng)使輸入變小。L的第五項(xiàng)確保偽輸入是正的[10]。通過引入這一項(xiàng)，可以唯一地確定滿足約束條件(式(21)～式(23))的解(fi,di)。

(2)最大夾持力函數(shù)：公式(7)中定義的函數(shù)Fmax,x(x,y)、Fmax,y(x,y)和Tmax(x,y)是不連續(xù)的。NMPC數(shù)值優(yōu)化中不連續(xù)函數(shù)的處理比較困難，因此將最大持力函數(shù)近似化為以下提出的連續(xù)函數(shù)：

(27)

(28)

(29)

為了滿足上述兩個(gè)目標(biāo)，引入了兩個(gè)新參數(shù)：σJ和σC。在每個(gè)肢體產(chǎn)生的夾持力最大值fmaxi,x,fmaxi,y、扭矩最大值τmaxi以及勢函數(shù)W表示如下：

(30)

(31)

(32)

(33)

(3)視距評(píng)估：如果墻體上有許多支撐，則最大持力函數(shù)(式(27)～式(29))會(huì)導(dǎo)致相當(dāng)高的計(jì)算成本。這是因?yàn)殡S著支撐數(shù)量的增加，最大保持力函數(shù)變得復(fù)雜。為了避免這個(gè)問題，引入了視距評(píng)估。

假設(shè)NMPC的時(shí)域長度小于從當(dāng)前時(shí)間到機(jī)器人完成攀登的時(shí)間長度。在這種情況下，運(yùn)動(dòng)只能預(yù)測和優(yōu)化到特定的未來時(shí)間。因此，即使使用所有的墻體信息來優(yōu)化運(yùn)動(dòng)，其中一些信息也不會(huì)影響最優(yōu)解。此外，由于NMPC本身是一種計(jì)算量大的控制方法，因此很難將視界長度延長到機(jī)器人完成攀爬的時(shí)間。注意到，即使在有限的信息范圍內(nèi)，也可以根據(jù)所有的墻體信息計(jì)算出控制輸入。這個(gè)范圍被定義為“視距”范圍。除了時(shí)域長度外，它還取決于模型的狀態(tài)或參數(shù)。在本研究中，以機(jī)器人的中心坐標(biāo)Q0、速度v和相對(duì)姿態(tài)角φ作為確定視距評(píng)估的參數(shù)。這樣，通過減少需要評(píng)估的信息量來減少計(jì)算時(shí)間。

為了將支撐限制在視距范圍內(nèi)，將式(28)～式(29)中的最大夾持力函數(shù)替換為：

(34)

(35)

(36)

式中，I為由視距范圍內(nèi)的支撐組成的評(píng)估集。從定義來看，I?H。讓視距范圍成為以機(jī)器人Q0為中心的圓形范圍。評(píng)估半徑由該圓的半徑表示。兩種墻體模型如圖3所示。左邊的例子是式(27)～式(29)中最大夾持力函數(shù)的一個(gè)示例，它用連續(xù)函數(shù)來近似。右邊的是式(34)～式(36)中最大保持力函數(shù)的一個(gè)示例，它僅限于視距范圍。

(a) 具有連續(xù)函數(shù)的墻體模型 (b) 具有視距范圍的墻體模型圖3 墻體模型

隨著函數(shù)的變化，在每個(gè)肢體產(chǎn)生的夾持力最大值fmaxi,x,fmaxi,y、扭矩最大值τmaxi以及勢函數(shù)W替換如下：

(37)

(38)

(39)

(40)

3 實(shí)驗(yàn)及結(jié)果分析

3.1 仿真設(shè)置

利用計(jì)算機(jī)數(shù)學(xué)軟件Maple，在Visual Studio 14.0編譯環(huán)境中使用C語言對(duì)提出的控制方法進(jìn)行了機(jī)器人行為模擬。

表1列出了與機(jī)器人相關(guān)的參數(shù)。這些參數(shù)都是無量綱的。機(jī)器人在仿真開始時(shí)的初始姿態(tài)如表2所示，此時(shí)機(jī)器人是靜止的，機(jī)器人的中心坐標(biāo)Q0位于原點(diǎn)(0,0)。假設(shè)此姿勢為穩(wěn)定姿勢，則性能指標(biāo)中參考姿勢角φref的值將設(shè)置為與初始姿勢角度相同的值。與墻體相關(guān)的參數(shù)如表3所示，與性能指標(biāo)相關(guān)的參數(shù)(例如權(quán)重)如表4所示。

表1 機(jī)器人參數(shù)

表2 初始姿勢參數(shù)

表3 墻體參數(shù)

表4 性能指標(biāo)參數(shù)

初始姿態(tài)和支撐布置情況如圖4中Time0所示。仿真的計(jì)算條件如表5所示，參考了文獻(xiàn)[10]的設(shè)置經(jīng)驗(yàn)。

圖4 攀爬行為結(jié)果(Case 4)

表5 仿真的計(jì)算條件

3.2 仿真結(jié)果

從圖4可以看出，機(jī)器人能夠根據(jù)墻體信息實(shí)現(xiàn)支撐選擇和爬墻支撐切換，驗(yàn)證了提出控制方法的可行性。Case 1、Case 2、Case 3和Case 4更新一次控制輸入的計(jì)算時(shí)間分別為12.4、15.2、18.5和19.5 ms。這些結(jié)果表明，隨著評(píng)估半徑的減小，計(jì)算時(shí)間也隨之減少。這是因?yàn)橛糜诜抡娴膲w信息量隨著評(píng)估半徑的減少而減少。

3.3 真實(shí)案例測試

選擇近藤公司的KHR-3HV型仿人機(jī)器人對(duì)提出控制方法進(jìn)行了實(shí)際攀爬測試。該仿人機(jī)器人沒有特殊的抓握機(jī)構(gòu)，而是采用角托架作為手，符合本文模型設(shè)定。此外，設(shè)計(jì)了一個(gè)攀爬的簡易墻體，由多孔木墻制成。支撐物是由金屬環(huán)制成的。同仿真實(shí)驗(yàn)一致，也使用了不同的評(píng)估半徑來執(zhí)行仿人攀爬機(jī)器人行為控制。同仿真一樣，圖5給出了Case 4的實(shí)際行為結(jié)果。每個(gè)Case均執(zhí)行10次并取平均獲得攀爬全過程的時(shí)間。結(jié)果顯示，Case 1、Case 2、Case 3和Case 4的時(shí)間排序與仿真結(jié)果一致。相比于Case 4(154 s)，Case 1(97.9 s)的攀爬時(shí)間減少了36.4%，驗(yàn)證了提出控制方法的有效性。

圖5 實(shí)測攀爬行為結(jié)果

4 結(jié)論

本文提出了一種基于NMPC的仿人機(jī)器人攀爬控制方法。該方法能夠在考慮機(jī)械約束的情況下實(shí)現(xiàn)路徑和運(yùn)動(dòng)的綜合優(yōu)化，并通過使用視距評(píng)估在線配置性能指標(biāo)來減少計(jì)算時(shí)間。與將墻體上的所有支撐都納入視距范圍的情況相比，攀爬時(shí)間減少了36.4%。使用狀態(tài)相關(guān)權(quán)重的性能指標(biāo)不僅可以應(yīng)用于機(jī)器人的攀爬控制，還可以應(yīng)用于其他NMPC問題。此外，該視距評(píng)估方法還可以應(yīng)用于具有復(fù)雜環(huán)境下的機(jī)器人或者車輛控制問題。但是，真實(shí)實(shí)驗(yàn)中每種Case的計(jì)算時(shí)間都略長于控制輸入的更新周期。后續(xù)將考慮使用高性能的計(jì)算機(jī)或開發(fā)一種新的方法來更有效地確定評(píng)估范圍以便解決這個(gè)問題。