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

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

0 引言

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

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

圖1 機器人直線行走方式

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

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

1 水下爬游機器人整體結構及建模

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

圖2 水下爬游機器人整體結構

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

圖3 水下爬游機器人機體模型

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

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

(1)

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

圖4 機器人腿部結構

bcosθ1=D

(2)

解得θ2的值為：

(3)

同樣的方法求解θ4，

(4)

(5)

則：

θ3=π-θ4

(6)

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

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

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

圖5 典型機械腿末端軌跡曲線

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

圖6 爬游機器人足端空間軌跡

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

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

(7)

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

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

(8)

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

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

(9)

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

(10)

末端速度為：

(11)

末端加速度為：

(12)

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

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

(13)

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

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

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

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

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

y=-128

(14)

(15)

(16)

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

圖7 爬游機器人足端軌跡曲線

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

圖8 爬游機器人足端加速度曲線

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

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

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

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

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

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

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

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

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

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

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

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

4 爬游機器人水池爬行運動實驗

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

圖13 爬游機器人前進爬行運動

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

圖14 機器人前進爬行運動時各腿關節(jié)角度

5 結束語

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