基于高階多項(xiàng)式的爬游機(jī)器人足端軌跡規(guī)劃

戴 禎，劉衛(wèi)東，徐景明，李 樂

(西北工業(yè)大學(xué) 航海學(xué)院，西安 710072)

0 引言

海洋是一個充滿未知、領(lǐng)域廣闊、可達(dá)性差的區(qū)域，近年來，人類對海洋的探索越來越深入。為了更好的認(rèn)識海洋，開發(fā)海洋資源，世界各國都在大力發(fā)展水下機(jī)器人技術(shù)。對比幾類典型機(jī)器人：輪式機(jī)器人在面對崎嶇地形時移動速率大大降低；履帶式機(jī)器人機(jī)動性差，在嚴(yán)重崎嶇地形下，容易發(fā)生側(cè)翻；而足式機(jī)器人在爬行運(yùn)動時與地面接觸點(diǎn)是一系列離散點(diǎn)，能夠適應(yīng)大多數(shù)非平坦結(jié)構(gòu)地形，具有較強(qiáng)的靈活性，且不易側(cè)翻[1-4]。因此，對具有海底爬行和水中巡游功能的水下爬游機(jī)器人的研究具有重要的意義。

水下爬游機(jī)器人能夠在海底進(jìn)行靈活爬行運(yùn)動，需要對機(jī)器人足端位置進(jìn)行軌跡規(guī)劃。軌跡規(guī)劃是采用時間序列信息對機(jī)器人足端的位置、速度等參數(shù)進(jìn)行規(guī)劃進(jìn)而控制機(jī)器人足端空間位置與姿態(tài)[5-8]。常用的軌跡規(guī)劃方法有貝塞爾曲線[9-10]、基于遺傳算法的軌跡曲線[11]和B樣條曲線[12]等。通過對機(jī)器人足端軌跡進(jìn)行規(guī)劃，使機(jī)械腿運(yùn)動平滑，減少沖擊和振動，對提高機(jī)械腿的穩(wěn)定性、可靠性和工作效率有重要意義。文獻(xiàn)[13] 為了解決機(jī)器人在運(yùn)動過程中機(jī)械腿與機(jī)體之間存在的互斥力問題，提出了一種當(dāng)機(jī)器人足端處于支撐相時，足端與跟關(guān)節(jié)的縱向距離保持恒定的直線行走方式，使機(jī)器人足端軌跡為平行于機(jī)體的直線軌跡，如圖1所示。而對于機(jī)器人足端處于擺動相時，如何在空間中運(yùn)動，并沒有進(jìn)行深入的研究。

圖1 機(jī)器人直線行走方式

文獻(xiàn)[14] 采用一種改進(jìn)B樣條曲線的方法進(jìn)行機(jī)器人足端軌跡規(guī)劃，該方法能夠有效提高機(jī)器人足端空間軌跡的平滑性，但所擬合的曲線不會經(jīng)過其控制點(diǎn)，因此控制精度不高。

本文以水下爬游機(jī)器人為研究對象，為了使機(jī)器人具有良好的水下爬行運(yùn)動能力，采用了一種直線和曲線相結(jié)合的機(jī)器人足端空間運(yùn)動軌跡；利用四階多項(xiàng)式和六階多項(xiàng)式分別對水下爬游機(jī)器人足端軌跡進(jìn)行規(guī)劃，由于六階多項(xiàng)式所規(guī)劃的軌跡曲線包含其加速度約束比四階多項(xiàng)式所規(guī)劃的軌跡曲線包含的約束條件多，所以六階多項(xiàng)式更適合用于本文所提出的足端空間運(yùn)動軌跡規(guī)劃。通過對機(jī)器人足端速度以及加速度的約束，使機(jī)器人足端軌跡在直線和曲線連接處的速度連續(xù)，解決了機(jī)械腿在運(yùn)動過程中的抖動問題，使機(jī)械腿能夠柔順運(yùn)動。

1 水下爬游機(jī)器人整體結(jié)構(gòu)及建模

水下爬游機(jī)器人兼顧水中巡游和海底爬行運(yùn)動能力，爬游機(jī)器人能夠在復(fù)雜的海底環(huán)境中運(yùn)動，其運(yùn)動控制系統(tǒng)起到了至關(guān)重要的作用。運(yùn)動控制系統(tǒng)在接收到爬行運(yùn)動指令后，按照規(guī)劃的運(yùn)動步態(tài)解算出機(jī)器人各腿關(guān)節(jié)電機(jī)的角度，將解算結(jié)果通過多軸管理器發(fā)送至各關(guān)節(jié)電機(jī)，使機(jī)器人實(shí)現(xiàn)爬行運(yùn)動。圖2為水下爬游機(jī)器人整體結(jié)構(gòu)示意圖。

圖2 水下爬游機(jī)器人整體結(jié)構(gòu)

運(yùn)動學(xué)建模是進(jìn)行機(jī)器人軌跡規(guī)劃及運(yùn)動控制的基礎(chǔ)。根據(jù)水下爬游機(jī)器人的結(jié)構(gòu)，建立機(jī)器人的機(jī)體模型如圖3所示。圖中①、②、③、④分別表示爬游機(jī)器人的四條腿：①-左前腿、②-左后腿、③-右后腿、④-右前腿。機(jī)體坐標(biāo)系的原點(diǎn)是爬游機(jī)器人的機(jī)體重心，Xb正向指向機(jī)器人前進(jìn)方向，Yb正向指向右移方向，Zb正向垂直XbOYb平面豎直向上。d1表示前腿/后腿沿OYb軸偏移量，d2表示前腿/后腿沿OXb軸偏移量。

圖3 水下爬游機(jī)器人機(jī)體模型

根據(jù)水下爬游機(jī)器人模型，對其進(jìn)行運(yùn)動學(xué)分析，利用數(shù)學(xué)方法推導(dǎo)出爬游機(jī)器人的正、逆運(yùn)動學(xué)解[15-16]，為水下爬游機(jī)器人運(yùn)動控制奠定基礎(chǔ)。

本文主要研究水下爬游機(jī)器人足端軌跡規(guī)劃，首先需要求解機(jī)器人逆運(yùn)動學(xué)。所謂逆運(yùn)動學(xué)，即已知機(jī)器人足端在機(jī)體坐標(biāo)系XbOYb下的位置坐標(biāo)，反解出各腿關(guān)節(jié)角的過程[17]。已知腿部足端在機(jī)體坐標(biāo)系下的坐標(biāo)為(x,y,z)，在求解跟關(guān)節(jié)處轉(zhuǎn)角θ1的大小時，首先做出其單腿結(jié)構(gòu)圖，如圖4(a)所示，(x,y)表示足端在機(jī)體坐標(biāo)系XbOYb下的坐標(biāo)，D為腿在支撐走一步過程中形成的足端軌跡與跟關(guān)節(jié)保持的恒定間隔距離，由正切函數(shù)定義：

(1)

在求解肩關(guān)節(jié)處轉(zhuǎn)角θ2和膝關(guān)節(jié)處轉(zhuǎn)角θ3時，對機(jī)械腿進(jìn)行后視投影，得到在YbOZb平面的結(jié)構(gòu)圖，如圖4(b)所示，(y,z)為腿在該平面下的足端坐標(biāo)，b為足端在支撐走一步過程中，該腿跟關(guān)節(jié)與足端的連線距離，它與圖4(a)中D的關(guān)系是：

圖4 機(jī)器人腿部結(jié)構(gòu)

bcosθ1=D

(2)

解得θ2的值為：

(3)

同樣的方法求解θ4，

(4)

(5)

則：

θ3=π-θ4

(6)

2 水下爬游機(jī)器人足端軌跡規(guī)劃算法

水下爬游機(jī)器人足端軌跡規(guī)劃所要解決的是機(jī)器人在爬行時的單腿足端軌跡生成問題，規(guī)劃的意義在于既滿足機(jī)器人運(yùn)動的需要，又能根據(jù)機(jī)器人的結(jié)構(gòu)、運(yùn)動及動力特性對其運(yùn)動進(jìn)行約束，從而保證了機(jī)器人有序、穩(wěn)定的運(yùn)動。

通常機(jī)器人在運(yùn)動的過程中與地面接觸時會產(chǎn)生一定沖擊，為了保證機(jī)械腿擺動過程中具有良好的啟動和制動特性，減小足端與地面的沖擊力，所以運(yùn)動軌跡曲線除了規(guī)劃足端參考點(diǎn)的起始點(diǎn)和終止點(diǎn)的位置外，還需要規(guī)劃以上兩點(diǎn)的速度和加速度特性。以往機(jī)器人爬行過程中的足端軌跡一般采用典型的直線型軌跡或者拋物線型軌跡[18-20]，如圖5所示。但是直線型軌跡在機(jī)械腿的柔順控制方面表現(xiàn)較差，所以一般用拋物線型軌跡代替直線型軌跡，拋物線型軌跡不但解決了機(jī)械腿的柔順控制問題而且具有更好的躍障能力，但是拋物線型軌跡在與地面接觸或離開地面時足端會與地面產(chǎn)生一個切向力，會對機(jī)械腿關(guān)節(jié)和足端造成嚴(yán)重的機(jī)械磨損。

圖5 典型機(jī)械腿末端軌跡曲線

本文提出一種曲線和直線相結(jié)合的足端軌跡。 如圖6所示為機(jī)械腿運(yùn)動一個周期內(nèi)，足端在空間的運(yùn)動軌跡示意圖，當(dāng)機(jī)器人處于靜止?fàn)顟B(tài)時，足端端點(diǎn)為規(guī)劃軌跡的中點(diǎn)，x為前進(jìn)方向，z為豎直抬起方向，y方向?yàn)楹愣ㄖ?。為了更好的研究足端軌跡在空間中的特性，把圖6中的軌跡劃分為4個階段，分別為擺動相的抬起階段(AB)、擺動階段(BCD)、下落階段(DE)和支撐相的支撐平移階段(EA)，當(dāng)機(jī)器人進(jìn)行爬行運(yùn)動時，機(jī)械腿末端始終處于空間中規(guī)劃軌跡上的一點(diǎn)，并且滿足一定的速度、加速度約束條件。

圖6 爬游機(jī)器人足端空間軌跡

對于直線型軌跡，可以用簡單的三次多項(xiàng)式策略進(jìn)行規(guī)劃，其形式為：

s(t)=c0+c1t+c2t2+c3t3

(7)

在時間T內(nèi)足端從起點(diǎn)運(yùn)動到終點(diǎn)，在起始處施加約束條件s(0)=s′(0)=0，在終點(diǎn)處施加s(T)=s′(T)=0的約束。在t=0和t=T時，對上式求導(dǎo)得：

s′(t)=c1+2c2t+3c3t2

(8)

通過以上4個約束條件求得多項(xiàng)式系數(shù)：c0=0，c1=0，c2=3/T2，c3=-2/T，但是本文所設(shè)計的足端軌跡存在曲線線段，需要確定一個中間點(diǎn)，如圖6中的C點(diǎn)，所以采用四階多項(xiàng)式來描述軌跡曲線：

s(t)=c0+c1t+c2t2+c3t3+c4t4

(9)

四階多項(xiàng)式可以由起始點(diǎn)位置和速度，終止點(diǎn)位置和速度以及中間點(diǎn)位置唯一確定。但爬游機(jī)器人腿部控制還需要考慮軌跡起止點(diǎn)和終止點(diǎn)的加速度約束，所以選用六階多項(xiàng)式來求解軌跡曲線，其數(shù)學(xué)表達(dá)為：

(10)

末端速度為：

(11)

末端加速度為：

(12)

當(dāng)機(jī)器人足端處于擺動相時，假設(shè)所需要的時間是tw，起始抬起時刻為t0=0，向前擺動和豎直下落時刻為t1=tw/4、t2=3tw/4，足端拋物線擺動所用時間為t=tw/2。

以豎直抬起階段AB段為例，六次多項(xiàng)式軌跡曲線求解方程：

(13)

然后利用軌跡曲線約束條件，即起止時刻的位置s(t0)、s(t1)，速度v(t0)、v(t1)，加速度a(t0)、a(t1)以及中間時刻的位置s(t1/2)就可以唯一確定一組解c0、c1、c2、c3、c4、c5、c6。

3 水下爬游機(jī)器人足端軌跡仿真與分析

針對本文提出的機(jī)器人足端軌跡，采用高階多項(xiàng)式對其進(jìn)行仿真分析，然后對多段軌跡連接點(diǎn)處的速度問題以及機(jī)器人足端抬起和下落過程中機(jī)械腿的抖動問題進(jìn)行分析，通過設(shè)置相關(guān)運(yùn)動參數(shù)，優(yōu)化機(jī)器人足端軌跡速度以及加速度特性，使機(jī)器人在爬行過程中具有良好的運(yùn)動控制性能。

3.1 機(jī)器人足端軌跡規(guī)劃仿真分析

本文所采用的水下爬游機(jī)器人足端軌跡中每一段都相對獨(dú)立，當(dāng)約束條件中每一段的起始點(diǎn)速度和加速度，終止點(diǎn)速度和加速度都為0時，根據(jù)機(jī)械腿結(jié)構(gòu)和其相關(guān)運(yùn)動參數(shù)，求得爬游機(jī)器人爬行一步的軌跡曲線方程，如式(14)、(15)和(16)：

y=-128

(14)

(15)

(16)

利用上述軌跡曲線方程，畫出爬游機(jī)器人足端軌跡曲線如圖7(a)所示，圖7(b)為利用四階多項(xiàng)式求解得到的機(jī)器人足端空間軌跡曲線。對比圖7(a)、圖7(b)中所示曲線并不能明顯看出六階多項(xiàng)式規(guī)劃得到曲線的優(yōu)勢，分別對圖7(a)、圖7(b)中機(jī)器人足端軌跡曲線求二階導(dǎo)，得到爬游機(jī)器人足端在空間中的加速度曲線，如圖8(a)、8(b)所示。由于y分量在整個運(yùn)動過程中保持恒定不變，所以只對軌跡z分量和x分量的加速度進(jìn)行討論。

圖7 爬游機(jī)器人足端軌跡曲線

對比分析圖8(a)、圖8(b)，在整個運(yùn)動過程中，利用六階多項(xiàng)式規(guī)劃得到的曲線加速度比四階多項(xiàng)式規(guī)劃得到的曲線加速度更光滑，而且加速度不會發(fā)生突變，這為機(jī)器人的柔順控制奠定了基礎(chǔ)。另外，在整個運(yùn)動結(jié)束時刻，由于六階多項(xiàng)式對機(jī)器人足端軌跡加速度存在約束作用，所以曲線加速度為0，而四階多項(xiàng)式求解得到的加速度是一個不等于0的值，這樣的加速度值會影響到下一階段的運(yùn)動控制，會使機(jī)械腿產(chǎn)生抖動，進(jìn)而影響機(jī)器人整體運(yùn)動控制效果。

圖8 爬游機(jī)器人足端加速度曲線

綜上所述，六階多項(xiàng)式不但可以約束機(jī)器人足端軌跡位置，還可以約束軌跡的速度以及加速度，因此，更適合本文所提出的水下爬游機(jī)器人足端軌跡規(guī)劃。

六階多項(xiàng)式所規(guī)劃得到的軌跡曲線能夠滿足本文所研究的機(jī)器人足端軌跡曲線的性能要求。但結(jié)合圖7(a)和圖8(a)分析可知，首先在圖7(a)中機(jī)器人足端軌跡曲線在z方向存在抖動；其次由于每執(zhí)行完一段獨(dú)立的軌跡，機(jī)器人足端速度就會為0。當(dāng)機(jī)器人足端從A點(diǎn)運(yùn)動到B點(diǎn)時速度就會為0，從B點(diǎn)經(jīng)過C點(diǎn)運(yùn)動到D點(diǎn)，整個過程中起始點(diǎn)和終止點(diǎn)機(jī)械腿末端的速度都為0，所以在圖8(a)中，加速度曲線連續(xù)上下波動。如果機(jī)械腿末端在AB段運(yùn)動結(jié)束之后保持勻速進(jìn)入BCD段，在B點(diǎn)附近加速度則保持為0不變化。根據(jù)牛頓第二定律F=ma，當(dāng)加速度為0時，機(jī)械腿關(guān)節(jié)電機(jī)力矩也等于0，這將意味著在整個運(yùn)動過程中機(jī)械腿所消耗得能量將會減小。同時會消除兩段軌跡連接處加速度連續(xù)波動問題，為機(jī)械腿的柔順控制提供良好的基礎(chǔ)。

3.2 機(jī)器人足端軌跡優(yōu)化仿真分析

考慮到機(jī)器人在海底爬行運(yùn)動時，會遇到各種復(fù)雜地形，為了使機(jī)器人適應(yīng)不同地形，機(jī)械腿在抬起階段速度快，而下落階段速度緩慢，防止機(jī)器人足端與地面發(fā)生剛性接觸。

根據(jù)以上分析，對機(jī)器人足端軌跡進(jìn)行優(yōu)化，重新設(shè)定機(jī)器人足端軌跡直線與曲線連接點(diǎn)處的約束條件為：v=vt(vt≠0)、a=0，同時使機(jī)器人足端抬起階段，即AB段時間縮短，而下落階段，即DE段時間增加，得到如圖9所示為優(yōu)化后的爬游機(jī)器人足端軌跡曲線與未優(yōu)化的軌跡曲線圖。

圖9 優(yōu)化前后機(jī)器人足端軌跡曲線

如圖9所示，前4 s內(nèi)機(jī)器人足端處于擺動相，優(yōu)化后的軌跡曲線中消除了z分量的波動，而且比未優(yōu)化的軌跡曲線提前到達(dá)了指定位置，后4 s機(jī)器人處于支撐相，優(yōu)化后和優(yōu)化前的曲線沒有明顯差別。而優(yōu)化后的x分量相對于未優(yōu)化之前響應(yīng)時間縮短。對圖9中軌跡曲線x分量和z分量求導(dǎo)得到軌跡的速度曲線如圖10所示，對比分析圖10中優(yōu)化前和優(yōu)化后的速度曲線可知，優(yōu)化后z分量速度在機(jī)器人足端初始抬起階段比未優(yōu)化的速度值更大，而且在機(jī)器人足端擺動和下落階段其波動性明顯要小于優(yōu)化前的值，這會使得機(jī)械腿足端的速度控制更加柔順。另外，圖10中x分量在足端抬起階段相對于未優(yōu)化的軌跡曲線其響應(yīng)時間更短，而在下落階段其所用時間延長，從而為足端接觸地面過程中的運(yùn)動控制提供條件。

圖10 優(yōu)化前后機(jī)器人足端速度

對圖9中的機(jī)器人足端軌跡曲線求二階導(dǎo)，得到相應(yīng)加速度曲線如圖11所示，圖11中優(yōu)化后的z分量加速度在機(jī)器人足端抬起階段其值明顯大于未優(yōu)化的加速度值，而且在擺動階段和下落階段加速度波動的幅值明顯小于優(yōu)化前。相比較優(yōu)化前x分量的加速度曲線，優(yōu)化后的曲線響應(yīng)速度更快，這一現(xiàn)象符合前面所提到的機(jī)器人足端快抬慢落的運(yùn)動規(guī)律。

圖11 優(yōu)化前后機(jī)器人足端加速度

由以上分析可以得出結(jié)論，利用優(yōu)化后的六次多項(xiàng)式軌跡規(guī)劃方法所求解得到的足端軌跡曲線具有良好的運(yùn)動性能，為機(jī)器人運(yùn)動控制奠定了基礎(chǔ)。

根據(jù)D-H法則，結(jié)合爬游機(jī)器人機(jī)械腿結(jié)構(gòu)參數(shù)建立仿真模型，通過仿真得到機(jī)器人單腿足端在空間中的運(yùn)動軌跡，如圖12所示。圖12中機(jī)器人足端首先豎直抬起，之后呈拋物線向前擺動，接著豎直下落進(jìn)入支撐相，最后機(jī)器人足端向后平移，使機(jī)器人機(jī)體向前運(yùn)動，這符合本文所規(guī)劃的足端軌跡曲線。圖12中ABCDEA為完整一步的空間軌跡，而OB′C′DEO為機(jī)器人足端從初始位置抬起行走半步的空間軌跡。機(jī)器人從支撐狀態(tài)開始運(yùn)動和運(yùn)動結(jié)束時回到支撐狀態(tài)都需要運(yùn)動半步，保證機(jī)器人在運(yùn)動開始時和運(yùn)動結(jié)束時處于支撐狀態(tài)。

4 爬游機(jī)器人水池爬行運(yùn)動實(shí)驗(yàn)

基于以上所述機(jī)器人足端軌跡規(guī)劃方法，經(jīng)過仿真驗(yàn)證后，進(jìn)一步在深度為20 m的水池環(huán)境中利用機(jī)器人實(shí)物樣機(jī)進(jìn)行試驗(yàn)。試驗(yàn)中機(jī)器人進(jìn)行前進(jìn)爬行運(yùn)動，運(yùn)動過程如圖13所示，通過水面觀察和水下相機(jī)記錄，表明水下爬游機(jī)器人具有良好的爬行運(yùn)動能力，且在運(yùn)動過程中機(jī)械腿不再出現(xiàn)抖動，驗(yàn)證了本文所提出的基于高階多項(xiàng)式的機(jī)器人足端軌跡規(guī)劃算法的可行性和有效性。

圖13 爬游機(jī)器人前進(jìn)爬行運(yùn)動

機(jī)器人在水底進(jìn)行前進(jìn)爬行運(yùn)動時，其腿部各關(guān)節(jié)角度變化如圖14所示。定義機(jī)體中心為機(jī)器人參考原點(diǎn)，各機(jī)械腿順時針方向轉(zhuǎn)動為機(jī)械腿跟關(guān)節(jié)的正方向，圖14中，從起始點(diǎn)開始左前腿和右前腿跟關(guān)節(jié)角度都減小，而左后腿和右后腿跟關(guān)節(jié)角度都增大，說明機(jī)器人右前腿和左后腿處于擺動前進(jìn)狀態(tài)，左前腿和右后腿處于支撐向后狀態(tài)。肩關(guān)節(jié)和膝關(guān)節(jié)隨時間發(fā)生周期性變化以保證機(jī)器人足端在空間中始終處在同一平面內(nèi)運(yùn)動。

圖14 機(jī)器人前進(jìn)爬行運(yùn)動時各腿關(guān)節(jié)角度

5 結(jié)束語

為了對爬游機(jī)器人足端軌跡進(jìn)行合理規(guī)劃，本文首先根據(jù)爬游機(jī)器人的整體結(jié)構(gòu)研究并建立了機(jī)器人運(yùn)動學(xué)數(shù)學(xué)模型；然后分析了四階多項(xiàng)式和六階多項(xiàng)式對機(jī)器人足端軌跡曲線的可控性，運(yùn)用基于六階多項(xiàng)式的機(jī)器人足端軌跡規(guī)劃算法，解決了直線軌跡和曲線軌跡連接處的速度不連續(xù)問題，使機(jī)械腿在運(yùn)動過程中更加柔順；之后建立機(jī)械腿仿真模型，通過仿真驗(yàn)證了算法的有效性。最后利用爬游機(jī)器人實(shí)物樣機(jī)在水池中穩(wěn)定爬行運(yùn)動，驗(yàn)證了本文所提出的高階多項(xiàng)式機(jī)器人足端軌跡規(guī)劃方法的有效性。