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

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

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

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

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

1 足端軌跡規(guī)劃

1.1 軌跡規(guī)劃

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

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

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

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

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

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

1.2 機構描述及運動學分析

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

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

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

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

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

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

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

2.1 插值點的選取

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

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

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

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

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

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

2.2 五次多項式插補

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

給定邊界條件為

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

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

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

因此，Hermite多項式為

其中

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

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

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

表2 和3 分別為對三類插值點使用Hermite 多項式和差商估計得到的角加速度、角加加速度突變最大值以及角加速度、角加加速度減少率。以髖關節(jié)為例，等間距插值點角加速度減少率為38.93%，切比雪夫插值點角加速度減少率為20.53%，等時插值點角加速度減少率為7.83%。所以，三類插值點情形下，采用Hermite 多項式估計方法對髖膝關節(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 多項式估計方法的髖膝關節(jié)角加速度突變最大值分別為41.38和45.86 rad/s2。在3 種插值方法中，等時插值的角加速度突變最大值最??；雖然切比雪夫插值的角加加速度突變最大值最小，但和等時點插值差別不大。且角加速度對于運動的影響更大，因此使用等時插值和Hermite 多項式估計方法有利于減少電機的抖動和控制器的功耗。

3 仿真與實驗

3.1 單腿軌跡仿真與實驗

為了驗證實際應用效果，使用實驗室開發(fā)的四足機器人。圖7是簡化單腿與實際腿部之間的映射關系。根據幾何關系可知，四足機器人實際結構和簡化結構之間存在轉換關系

圖7 單腿映射關系Fig.7 Single-leg mapping

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

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

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

在實際曲線和理想的復合軌跡曲線上，各取500 個點用式（14）進行誤差計算，結果如表4 所示。可以看出，等時點插值的曲線與理想的復合軌跡的接近程度更好，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 多項式估計的等時點插值方法在一個擺動相運動周期內的運動過程。通過激光跟蹤儀來對足端進行跟蹤采集來獲得實際樣機的足端軌跡，如圖10 所示。由于實際樣機腿部關節(jié)處存在裝配間隙，導致激光跟蹤儀采集得到的足端軌跡和仿真得到的足端軌跡存在細小的偏差。但通過實體樣機實驗，可以看出機器人單腿運動平穩(wě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)仿真實驗

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

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

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

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

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

4 結 論

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

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

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