魏順祥，吳海波，陳久朋，劉亮，熊彬州，徐洋洋

1. 昆明理工大學(xué)機電工程學(xué)院，云南 昆明 650500

2. 珠海華冠科技股份有限公司，廣東 珠海 519000

仿生機器人是當(dāng)前研究的熱點問題，而良好的足端軌跡可以增加四足機器人對復(fù)雜地形的適應(yīng)性和穩(wěn)定性，因此四足機器人的軌跡規(guī)劃是研究的重要問題［1-3］。四足機器人的軌跡規(guī)劃通常是在笛卡爾空間或關(guān)節(jié)空間中進行的，并且兩者之間存在著映射關(guān)系。笛卡爾空間軌跡規(guī)劃能直觀了解足端的運動軌跡，但求解關(guān)節(jié)轉(zhuǎn)角的逆運動學(xué)計算量很大。關(guān)節(jié)空間軌跡規(guī)劃常采用插值的方法，在運算方面有著較好的優(yōu)越性［4-5］。常見的足端軌跡有擺線軌跡和多項式軌跡等方式［6-8］。Yoshihiro Sakakibara 等提出了擺線軌跡的規(guī)劃方法［9］。王立鵬等提出了足端零沖擊軌跡，使足端軌跡在理論上具有零沖擊的特性［10］。袁帥東等改進了擺線軌跡，在擺動相中間加了一條水平線，增強了足端的跨障能力［11］。為了使加速度連續(xù)并得到更平滑的足端軌跡曲線，Thomas Buschmann提出了基于分段五次多項式的足端軌跡［12］，該軌跡可在保證加速度曲線連續(xù)的同時，保證足端在起落點時實現(xiàn)理論上的零沖擊。鄧華等利用五次多項式對六足并聯(lián)機器人的足端軌跡進行規(guī)劃，取得了良好的效果［13］。周坤等利用五次多項式規(guī)劃了擺動腿跨步階段的足端軌跡，并在四足機器人上進行了實驗驗證［14］。常用的關(guān)節(jié)空間軌跡規(guī)劃方法有線性插值、B樣條插值法和高階多項式插值等［15］。線性插值法簡單方便，但其速度不連續(xù)且要求加速度無限大。高階多項式可保證關(guān)節(jié)空間運動過程中的角速度和角加速度連續(xù)。Reza Dehghani 等使用五次多項式插值進行了五連桿兩足動物的循環(huán)步態(tài)規(guī)劃和運動控制［16］。仲軍等根據(jù)主動力矩最小的原則，使用八階多項式對四足機器人關(guān)節(jié)空間進行了軌跡規(guī)劃和實驗驗證［17］。

理想情況下，可通過無限細(xì)分足端軌跡點數(shù)量進行逆解來計算關(guān)節(jié)角度，從而獲得關(guān)節(jié)運動的特性曲線來控制關(guān)節(jié)的精確運動。但由于控制器計算能力的限制，無法進行密集取點并進行運動學(xué)逆解計算。本文將笛卡爾空間和關(guān)節(jié)空間規(guī)劃的優(yōu)點相結(jié)合，對規(guī)劃好的足端軌跡進行離散采樣，使用不同的插值點選取方式確定較少的插值點位置進行運動學(xué)反解；并在關(guān)節(jié)空間內(nèi)進行角速度和角加速的優(yōu)化，再對相鄰的插補點之間用五次多項式函數(shù)關(guān)系進行連接［18］，以獲得平滑的關(guān)節(jié)曲線來驅(qū)動電機運動。因只需對足端軌跡曲線的采樣點進行逆運動學(xué)計算，本文的方法極大地減少了計算量。最后，將得到的足端軌跡與理想的足端軌跡進行了誤差比較，得到了最優(yōu)的插值方法和角速度、角加速度優(yōu)化方法。

1 足端軌跡規(guī)劃

1.1 軌跡規(guī)劃

擺線運動規(guī)律因能很好地滿足足端零沖擊的軌跡規(guī)劃要求，在四足機器人的足端軌跡規(guī)劃中得到了廣泛的應(yīng)用，其表達(dá)式為

式中H為抬腿高度，S為步幅，Tf為擺動相周期，Tω為擺動相周期的一半。

文獻［12］中，Thomas Buschmann 基于五次多項式構(gòu)造足端軌跡，使得該軌跡具備足端零沖擊以及速度、加速度連續(xù)的特點。相比于擺線運動規(guī)律，基于五次多項式構(gòu)造足端軌跡無泰勒級數(shù)展開求解的負(fù)擔(dān)。其數(shù)學(xué)表達(dá)式為

基于擺線類型的軌跡具備圓弧形的特點，方便在后續(xù)使用切比雪夫點進行插值點的選?。欢谖宕味囗検筋愋偷能壽E，在后續(xù)進行五次多項式關(guān)節(jié)空間軌跡規(guī)劃中具有連貫性和一致性。因此，本文選用了擺線軌跡X方向的曲線和五次多項式軌跡Y方向的曲線，建立了復(fù)合足端軌跡（以下簡稱復(fù)合軌跡），實現(xiàn)了將擺線運動規(guī)律和五次多項式運動規(guī)律的融合。該軌跡曲線既結(jié)合了擺線規(guī)律和五次多項式規(guī)律的優(yōu)點，又使得足端在理論上具有零沖擊、速度和加速度連續(xù)的特點，其形式為

給出一組運動參數(shù)H= 40 mm，S= 160 mm，Tf= 0.5 s，Tw= 0.25 s。圖1 是根據(jù)給定參數(shù)得出的笛卡爾坐標(biāo)空間的三類足端軌跡曲線。其中，復(fù)合軌跡具有更大的底部運動空間，落地角度更加垂直不易打滑，故選取復(fù)合軌跡作為四足機器人的足端軌跡。

圖1 笛卡爾坐標(biāo)空間的三類足端軌跡Fig.1 Three types of foot trajectories in Cartesian coordinate space

1.2 機構(gòu)描述及運動學(xué)分析

為了便于進行軌跡規(guī)劃研究，采用簡化模型進行運動學(xué)分析。如圖2所示，整機采用前肘后膝式配置。設(shè)大腿長為L1，小腿長為L2，髖關(guān)節(jié)轉(zhuǎn)角為θ1，膝關(guān)節(jié)轉(zhuǎn)角為θ2。

圖2 四足機器人簡化模型Fig.2 Simplified model of a quadruped robot

用Simulink 搭建上述簡化模型的單腿運動系統(tǒng)。通過對該系統(tǒng)發(fā)送一個時間信號激勵，使該模型按照設(shè)定的復(fù)合軌跡進行逆向運動學(xué)求解，并通過求解的結(jié)果驅(qū)動系統(tǒng)的物理模型進行軌跡的運動學(xué)仿真。取L1= 170 mm，L2= 255 mm，單腿一個步態(tài)周期的連桿結(jié)構(gòu)軌跡如圖3所示。

圖3 單腿一個步態(tài)周期的連桿結(jié)構(gòu)軌跡Fig.3 A gait cycle link structure trajectory of a single leg

在Simulink仿真中，獲得髖膝關(guān)節(jié)的位移、角速度和角加速度如圖4 所示。圖4 中，關(guān)節(jié)轉(zhuǎn)動的角度曲線平滑無突變，角速度、角加速度曲線在較小范圍內(nèi)平緩波動。上述關(guān)節(jié)運動曲線是微分細(xì)化后的求解結(jié)果，是對理想的關(guān)節(jié)運動曲線的逼近［19］。但在實際使用中，由于控制器計算能力有限，無法拾取大量中間點并進行運動學(xué)正反解計算，這將極大阻礙系統(tǒng)運動的實時性［20］。同時，微分的過程忽略了各個相鄰中間點之間的連接關(guān)系，僅是對求解后的點進行直線對接，會導(dǎo)致與實際運動情況不符［21-22］。

圖4 髖膝關(guān)節(jié)的位移、速度和加速度曲線Fig.4 Displacement,velocity and acceleration curves of hip and knee joint

2 關(guān)節(jié)空間軌跡優(yōu)化

2.1 插值點的選取

合適的插值點選擇可以將軌跡的中間點數(shù)量降到最少，增加軌跡與理想軌跡的接近程度。對足端軌跡進行離散插值的方法有等間距點、等時點及切比雪夫點等多種方法［23-24］。在軌跡平面內(nèi)，對x方向等分稱為等間距點；對時間t等分稱為等時點；切比雪夫點是單位圓周上等距分布的橫坐標(biāo)，在端點處更密集，更適合于圓弧曲線的插值。由于切比雪夫點可避免龍格現(xiàn)象的產(chǎn)生，因此被廣泛地用于高階多項式的插值。第二類切比雪夫多項式為

其中n∈N，x∈R，且|x| ≤1。式（4）也可記為

這里n∈N，x∈C。對式（5）求根，即可得第二類切比雪夫點。n階第二類切比雪夫多項式在區(qū)間[-1，1]內(nèi)有n+ 1個根，即

其中k= 0，1，2，…，n。在軌跡上選取11 個插值點將曲線分為10 份，表1 給出了等間距插值點、切比雪夫插值點以及等時插值點的坐標(biāo)值，與圖5中軌跡曲線上的插值點對應(yīng)。

表1 三種足端插值點位置Table 1 Three kinds of foot-end interpolation point position

圖5 三種足端插值曲線Fig.5 Three kinds of foot-end interpolation curves

2.2 五次多項式插補

為了確定三種插值對關(guān)節(jié)空間映射效果，將等間距點、切比雪夫點和等時點對應(yīng)的軌跡點作為選用的插值映射點進行運動學(xué)反解求得關(guān)節(jié)空間的軌跡插值點。這在很大程度上減輕了控制器的計算負(fù)擔(dān)，增強了系統(tǒng)運動的控制實時性。為保證各插值點之間平滑過渡，以達(dá)到相鄰點之間的角度、角速度、角加速度值連續(xù)的效果，需要在各相鄰插值點之間設(shè)置插補函數(shù)。五次多項式能滿足角度θ，角速度ω，角加速度α連續(xù)的最少階數(shù)要求，相比于更高階的多項式，計算量較小，還能夠避免龍格現(xiàn)象。所以，本文采用五次多項式曲線對各相鄰插值點進行連接，通過確定各插值點的關(guān)節(jié)特性參數(shù)，從而保證兩個插值點之間的平滑性。五次多次式的一般表達(dá)式為

給定邊界條件為

其中θ0，ω0，α0為起始點的角度、角速度和角加速度。θf，ωf，αf為終止點的角度、角速度和角加速度。將邊界條件代入（7），得

一般情況而言，式（8）中的θ0，θf是已知的，ω0，ωf，α0，αf可通過差商來確定。然而，差商的結(jié)果與實際偏差較大，所以本文構(gòu)造Hermite多項式估計關(guān)節(jié)插值點的角速度和角加速度，從而減少關(guān)節(jié)的沖擊和振動，提升機器人關(guān)節(jié)空間運動軌跡的平滑性。

首先，取前3 個插補點θ1，θ2，θ3和邊界條件ω1= 0 來構(gòu)造三次三節(jié)點的Hermite 多項式，即多項式P(t)滿足條件

因此，Hermite多項式為

其中

第二個插值點的角速度和角加速度估計值為ω2，α2。同理，ωi，αi可以由θi-1，θi，θi+1和已經(jīng)求出的ωi-1來確定，其中i= 2，3，…，9。從而，求出除首末兩點之外的所有插值點的角速度和角加速 度。 設(shè) 定ω與α的 首 末 邊 界 條 件 為ω1= 0，ω11= 0，α1= 0，α11= 0，就可得到全部關(guān)節(jié)插補點的角度、角速度、角加速度數(shù)據(jù)。

在給定插值點基礎(chǔ)上進行逆向運動學(xué)求解，得到11 個關(guān)節(jié)空間插值點，分別對三類插值點構(gòu)造三次三節(jié)點的Hermite 多項式來估計插值點的角速度和角加速度，并對其進行五次多項式軌跡規(guī)劃。圖6是采用切比雪夫點插值軌跡獲得的關(guān)節(jié)角速度、角加速度和角加加速度?？梢钥闯?，采用Hermite 多項式估計方法能有效的減少角加速度和角加加速度的突變值大小，角速度曲線也變得更加的平滑。

圖6 關(guān)節(jié)角速度、角加速度和角加加速度的對比分析Fig.6 Comparative analysis of joint angular velocity,angular acceleration and angular jerk

表2 和3 分別為對三類插值點使用Hermite 多項式和差商估計得到的角加速度、角加加速度突變最大值以及角加速度、角加加速度減少率。以髖關(guān)節(jié)為例，等間距插值點角加速度減少率為38.93%，切比雪夫插值點角加速度減少率為20.53%，等時插值點角加速度減少率為7.83%。所以，三類插值點情形下，采用Hermite 多項式估計方法對髖膝關(guān)節(jié)角加速度和角加加速度的突變最大值均有較大的減小。

表2 三類插值點的Hermite多項式和差商估計角加速度突變最大值、減少率Table 2 The Hermite polynomial and difference quotient estimation of three types of interpolation points to estimate the maximum angular acceleration abrupt change and reduction rate

此外，采用等時插值和Hermite 多項式估計方法的髖膝關(guān)節(jié)角加速度突變最大值分別為41.38和45.86 rad/s2。在3 種插值方法中，等時插值的角加速度突變最大值最?。浑m然切比雪夫插值的角加加速度突變最大值最小，但和等時點插值差別不大。且角加速度對于運動的影響更大，因此使用等時插值和Hermite 多項式估計方法有利于減少電機的抖動和控制器的功耗。

3 仿真與實驗

3.1 單腿軌跡仿真與實驗

為了驗證實際應(yīng)用效果，使用實驗室開發(fā)的四足機器人。圖7是簡化單腿與實際腿部之間的映射關(guān)系。根據(jù)幾何關(guān)系可知，四足機器人實際結(jié)構(gòu)和簡化結(jié)構(gòu)之間存在轉(zhuǎn)換關(guān)系

圖7 單腿映射關(guān)系Fig.7 Single-leg mapping

采用Simulink 對該機器人進行運動仿真分析，圖8是單腿擺動相的虛擬樣機運動過程。為了定量描述實際曲線和理想復(fù)合曲線的偏差大小，這里采用式（14）計算平均誤差值（以下簡稱為誤差值），即

圖8 虛擬樣機的運動過程Fig.8 The movement process of the virtual prototype

式中n代表實際曲線和理想復(fù)合曲線的取點個數(shù)，(?(ti)，?(ti))為實際曲線上的點，(x(ti)，y(ti))為理想曲線上的點。

在實際曲線和理想的復(fù)合軌跡曲線上，各取500 個點用式（14）進行誤差計算，結(jié)果如表4 所示?？梢钥闯?，等時點插值的曲線與理想的復(fù)合軌跡的接近程度更好，Hermite 多項式估計方法相比于差商估計對曲線的接近程度有明顯的提高；使用Hermite 多項式估計，等距點插值的誤差降低了12.61%，切比雪夫點插值的誤差降低了23.3%，等時點插值誤差降低了29.24%。

表4 兩種插值法的曲線誤差比較Table 4 Comparison of the curve error of the two interpolation methods

表3 三類插值點的Hermite多項式和差商估計角加加速度突變最大值、減少率Table 3 The Hermite polynomial and difference quotient estimation of three types of interpolation points to estimate the maximum angular jerk abrupt change and reduction rate

圖9 是實體樣機使用Hermite 多項式估計的等時點插值方法在一個擺動相運動周期內(nèi)的運動過程。通過激光跟蹤儀來對足端進行跟蹤采集來獲得實際樣機的足端軌跡，如圖10 所示。由于實際樣機腿部關(guān)節(jié)處存在裝配間隙，導(dǎo)致激光跟蹤儀采集得到的足端軌跡和仿真得到的足端軌跡存在細(xì)小的偏差。但通過實體樣機實驗，可以看出機器人單腿運動平穩(wěn)，無明顯抖動現(xiàn)象，證明了所提出算法的有效性和正確性。

圖9 實體樣機的運動過程Fig.9 The movement process of the physical prototype

圖10 激光跟蹤儀獲得的實際樣機足端軌跡Fig.10 Foot trajectory of actual prototype obtained by laser tracker

3.2 步態(tài)仿真實驗

為了驗證復(fù)合軌跡在四足機器人上的行走的效果，使用Matlab和Adams進行聯(lián)合仿真實驗。采用trot步態(tài)，設(shè)定步態(tài)周期為0.8 s，使用本文所提出的復(fù)合軌跡和優(yōu)化算法進行四足機器人整機的步態(tài)仿真。四足機器人一個步態(tài)周期的運動如圖11所示。

圖11 四足機器人一個步態(tài)周期運動Fig.11 A gait cycle of a quadruped robot

在仿真模型上應(yīng)用復(fù)合軌跡、擺線軌跡和五次多項式軌跡進行tort步態(tài)仿真實驗。軀干質(zhì)心在世界坐標(biāo)系上運動軌跡如圖12～13 所示。圖12 中，三種軌跡的四足機器人步態(tài)平穩(wěn)，無明顯打滑現(xiàn)象；且由放大圖可以看出，復(fù)合軌跡質(zhì)心位移最遠(yuǎn)，其速度為395.17 mm/s，更不易發(fā)生打滑。圖13 中，三種軌跡的四足機器人運動平穩(wěn)，軀干的起伏波動較小，且應(yīng)用三種軌跡的機器人質(zhì)心波動范圍差別很小。以上證明了所設(shè)計的復(fù)合軌跡和相關(guān)算法的可行性。

圖12 軀干質(zhì)心在前進方向上的運動軌跡Fig.12 The trajectory of the torso center of mass in the forward direction

圖13 軀干質(zhì)心在豎直方向上的運動軌跡Fig.13 The trajectory of the torso center of mass in the vertical direction

4 結(jié) 論

針對四足機器人的笛卡爾空間足端軌跡規(guī)劃，研究了擺線運動規(guī)律和五次多項式運動規(guī)律的性質(zhì)和特點，將兩者相結(jié)合得到了一種復(fù)合足端軌跡。為降低實際運動過程中足端軌跡的運動學(xué)計算負(fù)擔(dān)，將復(fù)合足端軌跡的插值點數(shù)量壓縮至11個，同時采取等間距點、切比雪夫點和等時點三類插值點來進行比較分析。

利用五次多項式曲線對各插值點之間進行關(guān)節(jié)空間軌跡規(guī)劃，使用Hermite 多項式曲線進行對插值點的角速度和角加速度估計，得到了基于三類插值方法的關(guān)節(jié)空間運動曲線。結(jié)果表明：使用Hermite 多項式估計的關(guān)節(jié)空間各關(guān)節(jié)的角速度變化更加平滑，角加速度和角加加速度的突變值明顯減少。對三類插值方法而言，等時點插值的角加速度突變值更低，效果更好。

通過Simulink 進行單腿仿真，并使用Hermite多項式估計，所得到的足端實際軌跡與理想的復(fù)合軌跡的誤差值明顯減少。采用等時點進行插值的方法所得到的實際軌跡與理想的復(fù)合足端軌跡更為接近。通過實體樣機實驗和整機的步態(tài)仿真實驗，驗證了上述方法和結(jié)論的有效性和正確性。