基于擴(kuò)展雅可比矩陣的冗余液壓驅(qū)動(dòng)四足機(jī)器人運(yùn)動(dòng)控制

陳光榮,郭 盛,侯博文,王軍政

(1.北京交通大學(xué)機(jī)械與電子控制工程學(xué)院,北京 100044;2.北京市軌道交通線路安全與防災(zāi)工程技術(shù)研究中心,北京 100044;3.北京理工大學(xué)自動(dòng)化學(xué)院,北京 100081)

1 引言

目前,機(jī)器人和機(jī)械臂的研究備受關(guān)注[1–2].為了實(shí)現(xiàn)更多樣化的功能和執(zhí)行更復(fù)雜化的任務(wù),越來越多的機(jī)器人和機(jī)械臂被設(shè)計(jì)成具有冗余自由度結(jié)構(gòu),如線驅(qū)動(dòng)超冗余機(jī)械臂[3]、仿人靈巧臂[4]、6R機(jī)器人[5]和冗余四足機(jī)器人[6]等.

冗余機(jī)構(gòu)的引入提高了機(jī)器人和機(jī)械臂的功能性和容錯(cuò)能力的同時(shí),也帶來了冗余機(jī)器人逆運(yùn)動(dòng)學(xué)求解[7–8]和奇異值規(guī)避[9]的問題,提高了控制上的難度.針對(duì)此問題,國內(nèi)外眾多學(xué)者開展了研究.經(jīng)典的運(yùn)逆動(dòng)學(xué)求解算法可分為解析法和優(yōu)化法,具體有代數(shù)法、幾何法、奇異值分解算法(singular value decomposition,SVD)、牛頓–拉普森方法、旋量理論法、廣義逆法、梯度投影法、擴(kuò)展雅可比矩陣法和智能優(yōu)化算法等.Li Shuai 等人提出利用神經(jīng)網(wǎng)絡(luò)的方法進(jìn)行冗余機(jī)器人的逆運(yùn)動(dòng)學(xué)求解,取得了一系列研究成果[10–12],并將運(yùn)用拓展到含噪聲冗余度機(jī)器人分布式網(wǎng)絡(luò)中的協(xié)同運(yùn)動(dòng)生成[13];基于動(dòng)態(tài)神經(jīng)網(wǎng)絡(luò)和非凸約束的冗余度機(jī)器人廣義重復(fù)運(yùn)動(dòng)規(guī)劃方案[14],從優(yōu)化視角研究冗余度機(jī)器人新的干擾抑制約束[15]等.Zhang Zhijun等人對(duì)冗余度機(jī)器人重復(fù)運(yùn)動(dòng)規(guī)劃方案提出3種遞歸神經(jīng)網(wǎng)絡(luò)和3種數(shù)值方法[16];對(duì)復(fù)雜路徑規(guī)劃中雙冗余度機(jī)器人協(xié)調(diào)運(yùn)動(dòng)提出了三準(zhǔn)則優(yōu)化方法[17];對(duì)冗余度機(jī)器人關(guān)節(jié)角漂移問題提出了變參數(shù)收斂微分神經(jīng)網(wǎng)絡(luò)[18].Guo Dongsheng等人研究了基于偽逆的冗余度機(jī)器人噪聲環(huán)境下PID路徑規(guī)劃新方案[19].Boudreau R等人則利用優(yōu)化的方法來求解余機(jī)器人的逆運(yùn)動(dòng)學(xué),目的是最小化驅(qū)動(dòng)力矩[20];此外,各種智能優(yōu)化算法也成為了冗余機(jī)器人的逆運(yùn)動(dòng)學(xué)求解的重要手段,比如蜂群、蟻群、粒子群、螢火蟲算法[21]和遺傳算法[22]等.雖然以上幾類方法能有效的解決冗余機(jī)器人逆運(yùn)動(dòng)學(xué)求解和奇異值規(guī)避問題,但是同時(shí)也帶來了計(jì)算量的提升,對(duì)控制器的計(jì)算性能要求變高,增加了成本和計(jì)算時(shí)間.

由于液壓驅(qū)動(dòng)具有精度高、響應(yīng)快、負(fù)載能力強(qiáng)等優(yōu)點(diǎn)[23–25],而四足機(jī)器人具有穩(wěn)定性好,動(dòng)態(tài)特性高等特點(diǎn)[26],液壓四足機(jī)器人得到了越來越多研究學(xué)者的青睞[27–28],而且液壓四足機(jī)器人中的很大一部分被設(shè)計(jì)成單腿具有四自由度的冗余結(jié)構(gòu)[29–30],如波士頓動(dòng)力的四足機(jī)器人Bigdog、中國北方車輛研究所的四足機(jī)器人“奔跑號(hào)”.雖然很多關(guān)節(jié)角度和關(guān)節(jié)力矩的優(yōu)化和約束被添加到冗余四足機(jī)器人逆運(yùn)動(dòng)學(xué)求解中,但目前還沒有學(xué)者綜合考慮冗余四足機(jī)器人入地角度、足端工作空間及零力矩點(diǎn)的協(xié)調(diào)等問題.

因此,針對(duì)運(yùn)動(dòng)學(xué)冗余的液壓四足機(jī)器人的逆運(yùn)動(dòng)學(xué)問題,本文提出了一種計(jì)算量少,簡(jiǎn)單易實(shí)現(xiàn)的擴(kuò)展雅可比矩陣法.此方法既能解決機(jī)器人運(yùn)動(dòng)學(xué)冗余問題,還能使機(jī)器人入地角度滿足摩擦錐的要求以避免足端滑動(dòng)并滿足零力矩點(diǎn)要求[31].通過仿真實(shí)驗(yàn)和常用的梯度投影法作了比較,此方法在對(duì)角步態(tài)(trot)下在實(shí)時(shí)性上和運(yùn)動(dòng)性能上有很好的表現(xiàn);在斜坡環(huán)境下也能平穩(wěn)的行走.最后又通過實(shí)驗(yàn)驗(yàn)證了該方法的可行性和有效性.

2 四足機(jī)器人模型

為了提高四足機(jī)器人的運(yùn)動(dòng)穩(wěn)定性,本文基于仿生學(xué)原理設(shè)計(jì)了液壓驅(qū)動(dòng)仿生四足機(jī)器人三維模型,如圖1所示,同時(shí)建立了機(jī)器人載體坐標(biāo)系{B},髖關(guān)節(jié)坐標(biāo)系{H}和足端坐標(biāo)系{F}.機(jī)器人每條腿共有5個(gè)自由度:4個(gè)主動(dòng)自由度和1個(gè)被動(dòng)伸縮自由度.4個(gè)主動(dòng)自由度由1個(gè)橫滾自由度和3個(gè)俯仰自由度組成,均由液壓缸伺服驅(qū)動(dòng).其中,髖關(guān)節(jié)有1個(gè)橫滾自由度和1個(gè)俯仰自由度,膝關(guān)節(jié)和踝關(guān)節(jié)各有1個(gè)俯仰自由度.髖關(guān)節(jié)橫滾自由度的主要作用是改變機(jī)器人行進(jìn)方向并抵抗側(cè)向干擾與沖擊.當(dāng)受到側(cè)向干擾與沖擊時(shí),機(jī)器人可通過側(cè)向?qū)遣綉B(tài)快速使機(jī)器人恢復(fù)姿態(tài)平衡和行走穩(wěn)定.1個(gè)被動(dòng)伸縮自由度由被動(dòng)伸縮彈簧構(gòu)成.被動(dòng)伸縮彈簧可有效減小地面對(duì)支撐腿的沖擊力,起到良好的緩沖和儲(chǔ)能作用,改善機(jī)器人的行走穩(wěn)定性和能源損耗.

四足機(jī)器人模型的結(jié)構(gòu)設(shè)計(jì)參數(shù)如表1所示.其中,仿真模型與實(shí)物樣機(jī)的結(jié)構(gòu)參數(shù)一致.

表1 四足機(jī)器人模型參數(shù)Table 1 Model parameters of quadruped robot

以四足機(jī)器人右前腿(right fore leg,RF)為例,以髖關(guān)節(jié)為原點(diǎn)建立前進(jìn)方向?yàn)閤軸,豎直方向?yàn)閦軸的符合右手原則的髖關(guān)節(jié)坐標(biāo)系{H},如圖2所示.

圖2 機(jī)器人單腿配置與坐標(biāo)系Fig.2 Configuration of single leg of quadruped robot

在圖2中,θ1為髖關(guān)節(jié)側(cè)擺角,θ2為髖關(guān)節(jié)俯仰角,θ3為膝關(guān)節(jié)俯仰角,θ4為踝關(guān)節(jié)俯仰角,θ5為機(jī)器人足端末端桿件與豎直方向夾角,θin為足端入地角.以上各關(guān)節(jié)角度運(yùn)動(dòng)范圍限制如表2所示.注意,由于θ2,θ3,θ4的角度運(yùn)動(dòng)范圍限制,致使四足機(jī)器人不可能出現(xiàn)奇異位置或處于奇異位置附近,即在逆運(yùn)動(dòng)學(xué)解算時(shí),避免了考慮奇異值規(guī)避問題,不會(huì)出現(xiàn)無解和解的病態(tài)程度很高的情況.

表2 機(jī)器人腿部各關(guān)節(jié)角度運(yùn)動(dòng)范圍限制Table 2 The range of joint angle of leg of quadruped robot

3 足端軌跡規(guī)劃

四足機(jī)器人在行走過程中,每條腿均由支撐相和擺動(dòng)相組成,并在支撐相和擺動(dòng)相間來回切換,從而完成機(jī)器人的行走運(yùn)動(dòng).其中,擺動(dòng)相是指腿部從抬腿時(shí)足端離地到落腿時(shí)足端觸地期間的相位變化過程;支撐相是指腿部從落腿時(shí)足端觸地到再次抬腿時(shí)足端離地期間腿的相位變化過程.

由于改進(jìn)的擺線方程可實(shí)現(xiàn)機(jī)器人足端軌跡速度和加速度的連續(xù)變化,故本文在足端坐標(biāo)系{F}下,采用改進(jìn)的擺線方程對(duì)機(jī)器人足端軌跡進(jìn)行規(guī)劃.當(dāng)足端處于支撐相時(shí),其在豎直方向上沒有位移運(yùn)動(dòng),使用擺線方程規(guī)劃足端水平方向的運(yùn)動(dòng),引導(dǎo)足端的加減速.當(dāng)足端處于擺動(dòng)相時(shí),其在水平和豎直方向上均有位移運(yùn)動(dòng)和加減速過程.整體足端軌跡規(guī)劃為

式中:(xsp(t),ysp(t)),(xsw(t),zsw(t))分別是支撐相和擺動(dòng)相的步態(tài)規(guī)劃足端坐標(biāo);S是步幅長(zhǎng)度;Tsp,Tsw分別是支撐相和擺動(dòng)相周期;H是抬腿高度;Vz是足端落地過程中的最大速度;Tz1,Tz2?Tz1,?Tz2分別是機(jī)器人足端從抬腿高度H減小到零過程中所經(jīng)過的加速時(shí)間、勻速時(shí)間和減速時(shí)間,這3個(gè)值可自行設(shè)定或通過實(shí)驗(yàn)調(diào)節(jié),但須滿足大小關(guān)系:Tz1<如此一來,機(jī)器人足端在水平和豎直方的速度和加速度的變化都是連續(xù)可導(dǎo)的,不會(huì)產(chǎn)生足端速度和加速度的突變,使足端軌跡更加平滑,有效避免了機(jī)器人與地面的硬接觸,同時(shí)接觸力方向也容易滿足摩擦錐的要求.

4 逆運(yùn)動(dòng)學(xué)分析

因?yàn)橛腥哂嘧杂啥鹊拇嬖?冗余四足機(jī)器人在運(yùn)動(dòng)學(xué)上可以根據(jù)實(shí)際需求進(jìn)行多種性能優(yōu)化,使機(jī)器人足端的工作空間更大,運(yùn)動(dòng)性能和越障能力更強(qiáng),復(fù)雜環(huán)境適應(yīng)性更高.冗余度機(jī)器人的逆運(yùn)動(dòng)學(xué)分析和性能優(yōu)化方法有多種.本文通過設(shè)計(jì)自選的優(yōu)化函數(shù)構(gòu)建擴(kuò)展雅可比矩陣來對(duì)冗余四足機(jī)器人進(jìn)行性能優(yōu)化,并與基于梯度投影法的逆運(yùn)動(dòng)學(xué)分析方法進(jìn)行對(duì)比.

假設(shè)四足機(jī)器人軀干與地面平行,且只有直線運(yùn)動(dòng)沒有外擺運(yùn)動(dòng),則側(cè)擺角度θ1為0.根據(jù)機(jī)器人腿部幾何關(guān)系可解算出機(jī)器人足端與桿長(zhǎng)和關(guān)節(jié)角之間的運(yùn)動(dòng)學(xué)關(guān)系.由圖2可知,足端在髖關(guān)節(jié)坐標(biāo)系{H}下的坐標(biāo):

對(duì)式(2)兩邊同時(shí)求導(dǎo),可得雅可比矩陣為

其中:X[x z]T是機(jī)器人足端在髖關(guān)節(jié)坐標(biāo)系{H}下的坐標(biāo);θ[θ2θ3θ4]T是機(jī)器人腿部各關(guān)節(jié)角度;雅可比矩陣J其中:

由式(2)–(3)可知,式(3)中的雅可比矩陣為胖矩陣,方程(2)求解存在未知數(shù)個(gè)數(shù)大于方程個(gè)數(shù)情況(3>2),即存在冗余度.冗余四足機(jī)器人在運(yùn)動(dòng)過程中,其逆運(yùn)動(dòng)學(xué)求解中存在各關(guān)節(jié)角度多解現(xiàn)象.以下主要通過梯度投影法和擴(kuò)展雅可比矩陣法這兩種方法進(jìn)行逆運(yùn)動(dòng)學(xué)求解,并比較二者在解決冗余四足機(jī)器人逆運(yùn)動(dòng)學(xué)問題上的優(yōu)劣.擴(kuò)展雅可比矩陣法主要是通過增加性能優(yōu)化函數(shù)作為額外的約束方程,引入到式(3)中的雅可比矩陣,從而使方程存在唯一解,且滿足給定的性能優(yōu)化指標(biāo).

4.1 梯度投影法

式(3)給出了機(jī)器人足端速度和腿部各關(guān)節(jié)角速度之間的關(guān)系,設(shè)J+∈Rn×m是雅可比矩陣的廣義逆;k為自運(yùn)動(dòng)的放大系數(shù);(I ?J+J)是零空間梯度投影矩陣;?? ∈Rn為任意待優(yōu)化目標(biāo)函數(shù)?(θ)的梯度.則冗余四足機(jī)器人關(guān)節(jié)角速度反解為:

式中:J+JT(JJT)?1;右式第1項(xiàng)J+是冗余四足機(jī)器人的最小范數(shù)解,右式第2項(xiàng)k(I ?J+J)??是自運(yùn)動(dòng)項(xiàng),用來優(yōu)化機(jī)器人運(yùn)動(dòng)學(xué)各項(xiàng)性能指標(biāo).對(duì)冗余四足機(jī)器人自運(yùn)動(dòng)的優(yōu)化是通過調(diào)整放大系數(shù)k和待優(yōu)化目標(biāo)函數(shù)?(θ)的梯度??來實(shí)現(xiàn)的.針對(duì)運(yùn)算過程中可能出現(xiàn)的奇異點(diǎn)問題,可采用文獻(xiàn)[9]中所提的方法,引入阻尼系數(shù)λ,可規(guī)避奇異點(diǎn).易知,梯度投影法得到的關(guān)節(jié)角速度式(4)容易存在累計(jì)誤差,尤其是多次迭代計(jì)算會(huì)造成更大的累計(jì)誤差.

4.2 擴(kuò)展雅可比矩陣法

4.2.1 擴(kuò)展雅可比矩陣法

對(duì)式(1)基于改進(jìn)擺線方程規(guī)劃的四足機(jī)器人足端軌跡兩邊求導(dǎo),可得到機(jī)器人足端速度與腿部各關(guān)節(jié)角速度之間的關(guān)系.由式(3)可知,此方程組由兩個(gè)方程3個(gè)未知數(shù)組成,是不定方程組,有無數(shù)組解.若能找到機(jī)器人某個(gè)性能指標(biāo)函數(shù)??(θ),其對(duì)關(guān)節(jié)角度的梯度為

然后將式(3)和式(5)聯(lián)立得

再定義擴(kuò)展雅可比矩陣Je為:

可以看出擴(kuò)展雅可比矩陣Je是方陣,方程個(gè)數(shù)與未知數(shù)個(gè)數(shù)相等.同時(shí)文獻(xiàn)[1]也給出了性能指標(biāo)函數(shù)如何規(guī)避奇異值的方法.

在給定腿部各關(guān)節(jié)角度運(yùn)動(dòng)范圍條件下(表2),四足機(jī)器人腿部均處于非奇異位置,即,便能得到關(guān)節(jié)角度的唯一解,即可解得關(guān)節(jié)角度的解析解

4.2.2 入地角度性能指標(biāo)函數(shù)規(guī)劃

由圖2可知,角度θ2,θ3,θ4,θ5存在如下關(guān)系:

其中θ5與入地角度θin又存在如下關(guān)系;

而四足機(jī)器人足端與地面接觸時(shí),為了防止機(jī)器人足端與地面產(chǎn)生滑動(dòng),其足端必須滿足摩擦錐的要求.如圖3所示,設(shè)四足機(jī)器人足端與地面接觸力為F,當(dāng)接觸力的水平分量Fx超過地面的最大靜摩擦力時(shí),即Fx>μFz,Fx位于摩擦錐之外,其中μ為靜摩擦系數(shù),則足端與地面之間將會(huì)產(chǎn)生相對(duì)滑動(dòng).若僅考慮四足機(jī)器人足端受力情況和地形摩擦系數(shù),則可定義摩擦錐的角度為:αtan?1μ.為保證四足機(jī)器人足端不打滑,應(yīng)該保證接觸力F與豎直方向的夾角θ小于摩擦錐的角度α.

圖3 足端摩擦力與摩擦錐示意圖Fig.3 Configuration of single leg of quadruped robot

在實(shí)際運(yùn)動(dòng)過程中,受機(jī)器人足端軌跡影響,機(jī)器人足端末端桿件與地面接觸力并不共線,即機(jī)器人足端入地角度θin和接觸力F與豎直方向的夾角θ并不相等.故可在足端軌跡規(guī)劃中加入入地角度θin性能指標(biāo)約束,使機(jī)器人足端落地時(shí)在豎直方向上的受力較大,即可保證足端滿足摩擦錐要求.由于θ5的角度值約束不同,四足機(jī)器人足端的工作空間不同,θ510?和θ540?時(shí)足端在髖關(guān)節(jié)坐標(biāo)系{H}下的工作空間分別如圖4和圖5所示.由圖可知,單腿工作空間在θ5的某些給定值約束條件下變得比較小,在此類約束下進(jìn)行足端軌跡規(guī)劃將使步態(tài)的步長(zhǎng)S和步高H受限嚴(yán)重.如果使用單一固定的θ5進(jìn)行足端軌跡規(guī)劃一定程度上會(huì)大大削弱機(jī)器人的運(yùn)動(dòng)性能.因此,進(jìn)行入地角度約束時(shí)應(yīng)盡量避免以上弊端,采用時(shí)變的θ5.

圖4 θ5=10?時(shí)的足端工作空間Fig.4 Workspace of foot end effector when θ5=10?

圖5 θ5=40?時(shí)的足端工作空間Fig.5 Workspace of foot end effector when θ5=40?

四足機(jī)器人足端運(yùn)動(dòng)呈周期性步態(tài)運(yùn)動(dòng),故θ5的性能指標(biāo)函數(shù)給定也應(yīng)是周期函數(shù).綜上考慮,設(shè)

由式(9)得

其中:θ50是機(jī)器人一個(gè)步態(tài)開始時(shí)θ5的初始值;T是整個(gè)步態(tài)周期;φ5(S,H,β,μ)是θ5的周期性變化幅值,其大小受足端工作空間、步態(tài)參數(shù)S,H、地面坡度β及靜摩擦系數(shù)μ等影響.至此便可計(jì)算出擴(kuò)展雅可比矩陣Je,且是方陣.計(jì)算擴(kuò)展雅可比矩陣的行列式為

由于四足機(jī)器人腿部機(jī)械結(jié)構(gòu)和關(guān)節(jié)角度運(yùn)動(dòng)范圍限制(表2):θ3∈(0,π),故由于四足機(jī)器人腿部機(jī)械結(jié)構(gòu)和關(guān)節(jié)角度運(yùn)動(dòng)范圍限制.因此在機(jī)器人足端工作空間內(nèi),由足端軌跡規(guī)劃式(1)和θ5的性能指標(biāo)函數(shù)約束可以得到唯一的腿部各關(guān)節(jié)角度解析解,即θ2,θ3,θ4的值,從而完成冗余四足機(jī)器人腿部基于入地角度性能指標(biāo)函數(shù)規(guī)劃的逆運(yùn)動(dòng)學(xué)解算.

4.2.3 髖關(guān)節(jié)角度性能指標(biāo)函數(shù)規(guī)劃

同樣地,除了可以對(duì)θ5的性能指標(biāo)函數(shù)進(jìn)行約束,也可以對(duì)θ2,θ3,θ4進(jìn)行性能指標(biāo)函數(shù)約束.四足機(jī)器人在對(duì)角步態(tài)(trot)正常行走時(shí),主要是通過膝關(guān)節(jié)θ3和踝關(guān)節(jié)θ4的運(yùn)動(dòng)實(shí)現(xiàn)腿部擺動(dòng),髖關(guān)節(jié)θ2的運(yùn)動(dòng)幅度相對(duì)較小.因此,在此使用相同方法給出髖關(guān)節(jié)角度性能指標(biāo)函數(shù)規(guī)劃如下:

由式(9)得

其中:θ20是機(jī)器人一個(gè)步態(tài)開始時(shí)θ2的初始角度,φ2是θ2周期性變化幅值.從而可得其擴(kuò)展雅可比矩陣Je,且其行列式為

由于四足機(jī)器人腿部機(jī)械結(jié)構(gòu)和關(guān)節(jié)角度運(yùn)動(dòng)范圍限制(表2):θ4∈(0,π),故.因此在機(jī)器人足端工作空間內(nèi),由足端軌跡規(guī)劃式(1)和θ2的性能指標(biāo)函數(shù)約束可以得到唯一的腿部各關(guān)節(jié)角度解析解,即θ3,θ4,θ5的值,從而完成冗余四足機(jī)器人腿部基于髖關(guān)節(jié)角度性能指標(biāo)函數(shù)規(guī)劃的逆運(yùn)動(dòng)學(xué)解算.

5 仿真實(shí)驗(yàn)

本文利用SolidWorks(2016)軟件設(shè)計(jì)機(jī)器人結(jié)構(gòu)模型,并將其導(dǎo)入到Adams(2017)軟件中作為虛擬樣機(jī),如圖1所示.然后通過MATLAB(R2018b)和Adams聯(lián)合仿真驗(yàn)證算法的有效性.仿真參數(shù)如表3所示.

表3 仿真參數(shù)Table 3 Simulation parameters of quadruped robot

5.1 梯度投影法

在梯度投影法中,取參數(shù)k?1,優(yōu)化目標(biāo)函數(shù)?(θ)為簡(jiǎn)單的髖關(guān)節(jié)運(yùn)動(dòng)函數(shù),仿真結(jié)果如圖6所示,其中足端豎直向上線為接觸力,線的長(zhǎng)短反映了力的大小,箭頭反映了力得方向.

圖6 梯度投影法trot行走仿真Fig.6 Simulations of trot walking with gradient projection method

圖7為仿真中四足機(jī)器人右前腿各關(guān)節(jié)角度隨時(shí)間的變化曲線.由圖可知,θ2,θ3,θ4三個(gè)關(guān)節(jié)角均隨時(shí)間呈現(xiàn)周期性變化規(guī)律.θ2,θ4在規(guī)律變化的同時(shí),整體大小有微小幅度的增加;而θ3在規(guī)律變化的同時(shí),整體大小有明顯幅度的減小.此現(xiàn)象說明梯度投影法的逆運(yùn)動(dòng)學(xué)求解存在累計(jì)誤差影響.而且,整個(gè)仿真過程運(yùn)行速度較慢,花費(fèi)時(shí)間較長(zhǎng).

圖7 梯度投影法trot行走仿真機(jī)器人右前腿關(guān)節(jié)角度Fig.7 Joint angles of right fore leg of robot in simulations of trot walking with gradient projection method

5.2 擴(kuò)展雅可比矩陣法

以入地角規(guī)劃式(11)為性能優(yōu)化函數(shù)進(jìn)行擴(kuò)展雅可比矩陣法的逆運(yùn)動(dòng)學(xué)求解,并仿真驗(yàn)證,結(jié)果如圖8所示.圖9為仿真中四足機(jī)器人右前腿各關(guān)節(jié)角度隨時(shí)間的變化曲線.由圖可知,θ2,θ3,θ4三個(gè)關(guān)節(jié)角均隨時(shí)間呈現(xiàn)周期性變化規(guī)律,且整體大小保持恒定,不存在增大或減小趨勢(shì).而且,整個(gè)仿真過程運(yùn)行速度相對(duì)較快,花費(fèi)時(shí)間相對(duì)較短.

圖8 擴(kuò)展雅可比矩陣法trot行走仿真(θ5/θin)Fig.8 Simulations of trot walking with extended Jacobian matrix method(θ5/θin)

圖9 擴(kuò)展雅可比矩陣法trot行走仿真機(jī)器人右前腿關(guān)節(jié)角度(θ5/θin)Fig.9 Joint angles of right fore leg of robot in simulations of trot walking with extended Jacobian matrix method(θ5/θin)

同樣地,以髖關(guān)節(jié)角規(guī)劃式(14)為性能優(yōu)化函數(shù)進(jìn)行擴(kuò)展雅可比矩陣法的逆運(yùn)動(dòng)學(xué)求解,并仿真驗(yàn)證,結(jié)果如圖10所示.圖11為仿真中四足機(jī)器人右前腿各關(guān)節(jié)角度隨時(shí)間的變化曲線.由圖可知,θ2,θ3,θ4三個(gè)關(guān)節(jié)角也均隨時(shí)間呈現(xiàn)周期性變化規(guī)律,且整體大小也保持恒定,也不存在增大或減小趨勢(shì).而且,整個(gè)仿真過程運(yùn)行速度和花費(fèi)時(shí)間與以入地角規(guī)劃式(11)為性能優(yōu)化函數(shù)的方法相差無幾.

圖10 擴(kuò)展雅可比矩陣法trot行走仿真(θ2)Fig.10 Simulations of trot walking with extended Jacobian matrix method(θ2)

圖11 擴(kuò)展雅可比矩陣法trot行走仿真機(jī)器人右前腿關(guān)節(jié)角度(θ2)Fig.11 Joint angles of right fore leg of robot in simulations of trot walking with extended Jacobian matrix method(θ2)

5.3 機(jī)器人斜坡trot步態(tài)行走

Trot步態(tài)是四足動(dòng)物最常用的步態(tài),也是四足機(jī)器人研究最多的步態(tài).該步態(tài)具有較高的動(dòng)態(tài)性能,也具有一定的動(dòng)態(tài)穩(wěn)定性.四足機(jī)器人在平坦地面上trot步態(tài)行走時(shí),其質(zhì)心投影點(diǎn)通常落在腿部支撐區(qū)域中心附近,具有較高的相對(duì)運(yùn)動(dòng)穩(wěn)定性.但四足機(jī)器人在斜坡地形上采用完全相同的trot步態(tài)行走而不做任何步態(tài)修正時(shí),隨著坡度的增大,其質(zhì)心投影點(diǎn)離腿部支撐區(qū)域邊界線的距離越近,甚至跑出支撐區(qū)域,此時(shí)機(jī)器人的運(yùn)動(dòng)穩(wěn)定性越來越差,甚至失穩(wěn).雖然四足機(jī)器人能夠在質(zhì)心投影點(diǎn)偏離支撐區(qū)域時(shí),及時(shí)地交替對(duì)角支撐腿以達(dá)到動(dòng)態(tài)穩(wěn)定,但該調(diào)整會(huì)造成機(jī)器人軀干搖擺不定,大部分時(shí)間處于姿態(tài)控制階段,而不是行進(jìn)控制階段,不利于機(jī)器人穩(wěn)定行走.針對(duì)此問題,可以通過調(diào)整機(jī)器人腿部入地角度來改變支撐區(qū)域位置,使機(jī)器人質(zhì)心投影點(diǎn)更加靠近支撐區(qū)域中心以提高機(jī)器人運(yùn)動(dòng)穩(wěn)定性,如圖12所示.其本質(zhì)也是機(jī)器人trot步態(tài)在上下斜坡行走時(shí),可以通過調(diào)整機(jī)器人質(zhì)心前后移動(dòng),以保持其零力矩點(diǎn)投影更加靠近對(duì)角支撐腿足端連線,至少應(yīng)當(dāng)保證其零力矩點(diǎn)落在虛擬支撐區(qū)域內(nèi),即對(duì)角擺動(dòng)腿的落腳點(diǎn)和對(duì)角支撐腿的足端點(diǎn)組成的支撐區(qū)域內(nèi).

圖12 機(jī)器人斜坡trot步態(tài)行走足端入地角調(diào)整示意圖Fig.12 Adjustment of contact angle of foot when robot walks on a slope with trot gait

如圖12左所示,四足機(jī)器人在平坦地面上,通過前后側(cè)腿足端入地角的調(diào)整使機(jī)器人質(zhì)心投影點(diǎn)在支撐區(qū)域內(nèi)的位置發(fā)生了改變,其相對(duì)支撐區(qū)域?qū)蔷€交點(diǎn)O前移了S2,移動(dòng)到O1點(diǎn)上.同樣,四足機(jī)器人在斜坡地形上也可以通過調(diào)整足端入地角方式來實(shí)現(xiàn)機(jī)器人質(zhì)心投影點(diǎn)的移動(dòng)以提高運(yùn)動(dòng)穩(wěn)定性,如圖12右所示.其中,β為斜面坡度,h為機(jī)器人質(zhì)心相對(duì)斜坡高度,O點(diǎn)為機(jī)器人足端支撐區(qū)域的對(duì)角線交點(diǎn),O1點(diǎn)為機(jī)器人質(zhì)心在水平支撐區(qū)域的投影點(diǎn),O2點(diǎn)為O3點(diǎn)在水平支撐區(qū)域的投影點(diǎn),O3點(diǎn)為機(jī)器人質(zhì)心在斜坡上的投影點(diǎn),S1為O1和O2兩點(diǎn)之間的距離,S2為O和O2兩點(diǎn)之間的距離.在圖12左中,可通過機(jī)器人足端在機(jī)器人載體坐標(biāo)系中的坐標(biāo)計(jì)算出S2的值.在圖12右中,通過幾何關(guān)系可以得到S1的計(jì)算公式:S1htanβcosβ.四足機(jī)器人trot步態(tài)前行或上下坡時(shí),只要其零力矩點(diǎn)落在穩(wěn)定區(qū)域內(nèi),即可保證機(jī)器人的運(yùn)動(dòng)穩(wěn)定.故若可保證0 ≤|S1?S2|≤r,其中r是穩(wěn)定閾值,可由經(jīng)驗(yàn)設(shè)定,則可保證機(jī)器人質(zhì)心投影落在水平支撐區(qū)域?qū)蔷€交點(diǎn)附近.如此便可通過調(diào)整機(jī)器人足端入地角來移動(dòng)機(jī)器人質(zhì)心投影點(diǎn)在水平支撐區(qū)域的位置,實(shí)現(xiàn)在斜坡上穩(wěn)定行走的目的,雖然改方法一定程度上限制了足端的工作空間,使前后側(cè)腿足端工作空間變小,但只要滿足步態(tài)參數(shù)要求,且還可以通過足端入地角的改變實(shí)時(shí)調(diào)整足端工作空間,就能使四足機(jī)器人在斜面上穩(wěn)定行走.

在仿真參數(shù)表3的基礎(chǔ)上,搭建斜坡地面坡度β12?,設(shè)定前側(cè)兩腿的θ5030?,后側(cè)兩腿的θ5050?進(jìn)行四足機(jī)器人斜坡trot步態(tài)行走仿真,如圖13所示.可以看出,四足機(jī)器人前后側(cè)腿的入地角度明顯不同,且能在斜坡上穩(wěn)定行走.

因?yàn)樗淖銠C(jī)器人在走斜坡地形時(shí)對(duì)其前后側(cè)腿的足端入地角進(jìn)行了調(diào)整,故選取其四足機(jī)器人一對(duì)角腿觀察腿部各關(guān)節(jié)角度變化.右前腿(right front,RF)和左后腿(left hind,LH)的各關(guān)節(jié)角度變化如圖14所示,在第1個(gè)周期(0～0.4 s),四足機(jī)器人從下蹲姿態(tài)站立起來,所有關(guān)節(jié)角度逐漸增大;在第2個(gè)周期(0.4 s～0.8 s),四足機(jī)器人進(jìn)行前后側(cè)腿足端入地角調(diào)整,可以看出右前腿和左后腿個(gè)角度的變化明顯不同;前兩個(gè)周期后,四足機(jī)器人各腿關(guān)節(jié)角度均隨時(shí)間呈周期性變化,說明四足機(jī)器人在調(diào)整足端入地角后,支撐腿和擺動(dòng)腿周期交替進(jìn)行trot步態(tài)行走,且能有效保持運(yùn)動(dòng)穩(wěn)定性,驗(yàn)證了該方法的可行性和有效性.

圖14 擴(kuò)展雅可比矩陣法機(jī)器人斜坡trot步態(tài)行走仿真右前腿關(guān)節(jié)角度(θ5/θin)Fig.14 Joint angles of right fore leg of robot in simulations of trot walking on a slope with extended Jacobian matrix method(θ5/θin)

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

為了進(jìn)一步驗(yàn)證所提出基于擴(kuò)展雅可比矩陣的逆運(yùn)動(dòng)學(xué)分析方法的可行性和有效性,搭建了冗余液壓驅(qū)動(dòng)四足機(jī)器人實(shí)物樣機(jī),并進(jìn)行梯度投影和擴(kuò)展雅可比矩陣兩種方法的對(duì)比實(shí)驗(yàn),實(shí)驗(yàn)系統(tǒng)平臺(tái)參數(shù)與表1–3一致,控制器為PC104主板.

6.1 梯度投影法

與仿真相同,取參數(shù)k?1,優(yōu)化目標(biāo)函數(shù)?(θ)為簡(jiǎn)單的髖關(guān)節(jié)運(yùn)動(dòng)函數(shù),實(shí)驗(yàn)結(jié)果如圖15所示.

圖15 梯度投影法trot行走實(shí)驗(yàn)Fig.15 Experiments of trot walking with gradient projection method

圖16(a)給出四個(gè)周期(1.6 s～3.2 s)的四足機(jī)器人右前腿各個(gè)關(guān)節(jié)角度的變化曲線,圖中:實(shí)線為機(jī)器人右前腿的給定關(guān)節(jié)角度:gθ1為外擺角度,gθ2～gθ4分別為右前腿髖關(guān)節(jié)、膝關(guān)節(jié)、踝關(guān)節(jié)俯仰角度;圖中虛線為機(jī)器人右前腿的實(shí)際關(guān)節(jié)角度:tθ1～tθ4分別對(duì)應(yīng)gθ1～gθ4各關(guān)節(jié).由圖可知,實(shí)際關(guān)節(jié)角度跟蹤曲線與給定關(guān)節(jié)角度曲線不僅存在微小延時(shí),而且同仿真一樣,tθ2,tθ4在規(guī)律變化的同時(shí),整體大小有微小幅度的增加,而tθ3在規(guī)律變化的同時(shí),整體大小有較小幅度的減小.圖16(b)對(duì)應(yīng)給出4個(gè)周期(1.6 s～3.2 s)的四足機(jī)器人右前腿各個(gè)液壓缸伸出長(zhǎng)度的變化曲線,圖中:實(shí)線為機(jī)器人右前腿各液壓缸的給定伸出長(zhǎng)度:gcy1～gcy4分別對(duì)應(yīng)于給定gθ1～gθ4;圖中虛線為機(jī)器人右前腿各液壓缸的實(shí)際伸出長(zhǎng)度:tcy1～tcy4分別對(duì)應(yīng)gcy1～gcy4各缸.由圖可知,各液壓缸的伸出長(zhǎng)度與給定伸出長(zhǎng)度也不僅存在微小延時(shí),而且同各關(guān)節(jié)角度變化趨勢(shì)一樣,tcy2,tcy4在規(guī)律變化的同時(shí),整體大小有微小幅度的增加,而tcy3在規(guī)律變化的同時(shí),整體大小有較小幅度的減小.以上現(xiàn)象表明:梯度投影法的實(shí)驗(yàn)結(jié)果同仿真結(jié)果保持一致,說明梯度投影法的逆運(yùn)動(dòng)學(xué)求解存在累計(jì)誤差影響.而且,為了滿足所需的控制器計(jì)算時(shí)間,程序設(shè)定的控制周期相對(duì)較長(zhǎng).

圖16 梯度投影法trot行走實(shí)驗(yàn)機(jī)器人跟蹤性能Fig.16 Track performance in experiments of trot walking of robot with gradient projection method

6.2 擴(kuò)展雅可比矩陣法

以入地角規(guī)劃式(11)為性能優(yōu)化函數(shù)進(jìn)行擴(kuò)展雅可比矩陣法的逆運(yùn)動(dòng)學(xué)求解,并實(shí)驗(yàn)驗(yàn)證,結(jié)果如圖17所示.

圖17 擴(kuò)展雅可比矩陣法trot行走實(shí)驗(yàn)Fig.17 Experiments of trot walking with extended Jacobian matrix method

圖18(a)給出4個(gè)周期(1.6 s～3.2 s)的四足機(jī)器人右前腿各個(gè)關(guān)節(jié)角度的變化曲線,由圖可知,雖然實(shí)際關(guān)節(jié)角度跟蹤曲線與給定關(guān)節(jié)角度曲線存在微小延時(shí),但還是能穩(wěn)定的跟蹤給定值.圖18(b)對(duì)應(yīng)給出四個(gè)周期(1.6 s～3.2 s)的四足機(jī)器人右前腿各個(gè)液壓缸伸出長(zhǎng)度的變化曲線,由圖可知,雖然各液壓缸的伸出長(zhǎng)度與給定伸出長(zhǎng)度存在微小延時(shí),但還是能穩(wěn)定的跟蹤給定值.以上現(xiàn)象表明:擴(kuò)展雅可比矩陣法的實(shí)驗(yàn)結(jié)果同仿真結(jié)果保持一致,在此驗(yàn)證了擴(kuò)展雅可比矩陣法逆運(yùn)動(dòng)學(xué)求解的可行性和有效性.而且,控制器所需的計(jì)算時(shí)間較短,程序設(shè)定的控制周期也相對(duì)較短.

圖18 擴(kuò)展雅可比矩陣法trot行走實(shí)驗(yàn)機(jī)器人跟蹤性能Fig.18 Track performance in experiments of trot walking of robot with extended Jacobian matrix method

7 結(jié)論

本文研究一種基于擴(kuò)展雅可比矩陣的冗余液壓驅(qū)動(dòng)四足機(jī)器人逆運(yùn)動(dòng)學(xué)分析方法.本文的主要貢獻(xiàn)如下:

· 給出了改進(jìn)的足端軌跡規(guī)劃方法以實(shí)現(xiàn)trot步態(tài)發(fā)生器.

· 給出了冗余液機(jī)器人逆運(yùn)動(dòng)學(xué)分析的兩種方法:梯度投影法和本文所提出的擴(kuò)展雅可比矩陣法,并進(jìn)行了對(duì)比仿真.結(jié)果表明:梯度投影法存在累積誤差,關(guān)節(jié)角度變化整體上存在一定幅度的偏移,且仿真速度較慢,耗時(shí)較長(zhǎng);擴(kuò)展雅可比矩陣法則沒有明顯的累積誤差和關(guān)節(jié)角度變化的整體偏移,且仿真速度較快,耗時(shí)較短.

· 給出了入地角度和髖關(guān)節(jié)角度的性能優(yōu)化函數(shù),并進(jìn)行了仿真驗(yàn)證.同時(shí)利用入地角度性能優(yōu)化函數(shù)仿真實(shí)現(xiàn)了trot步態(tài)在斜坡上的穩(wěn)定行走.

· 最后通過實(shí)際樣機(jī)試驗(yàn)對(duì)本文所提的方法進(jìn)行了驗(yàn)證,在有負(fù)載的情況下機(jī)器人實(shí)際樣機(jī)能夠穩(wěn)定地在trot步態(tài)下行走,證明了該方法的可行性、有效性和實(shí)際應(yīng)用價(jià)值.

本文的研究可以運(yùn)用到其他冗余機(jī)器人或冗余機(jī)械臂.若沒有關(guān)節(jié)角度運(yùn)動(dòng)范圍限制,出現(xiàn)了奇異位置,則可以通過奇異值規(guī)避方法加以解決,保證算法的一般性和可推廣性.