，， ，，

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

0 引言

進(jìn)行水下探索，開發(fā)海洋資源已經(jīng)成為各國的主要目標(biāo)，傳統(tǒng)的ROV(Remote Operated Vehicle)由于自帶線纜的影響，使得它只能在一定區(qū)域內(nèi)進(jìn)行巡航，作業(yè)范圍大大受限；AUV(Autonomous Underwater Vehicle)依靠自身的自治能力來管理和控制自己完成使命，在實(shí)時(shí)通信上存在缺陷。此外，兩者最大的共同點(diǎn)是只能巡航作業(yè)，無法實(shí)現(xiàn)精確定位。文獻(xiàn)[1-4]提出了陸地六足機(jī)器人的設(shè)計(jì)與研究，但不能應(yīng)用于水下。基于此，研究了一種能在水下爬游相結(jié)合的機(jī)器人，如圖1所示。水下爬游機(jī)器人綜合了兩者各自的特點(diǎn)，既能水中巡航，又能在海底爬行，實(shí)現(xiàn)精確定位。它以巡游的方式到達(dá)作業(yè)區(qū)域后，再以爬行方式到達(dá)作業(yè)點(diǎn)，在探索地形，水下救援，打撈等方面具有獨(dú)特的優(yōu)勢。

針對(duì)機(jī)器人在海底爬行過程中需要解決的主要問題進(jìn)行探索：第一，機(jī)體兩側(cè)腿在處于支撐態(tài)行走過程中出現(xiàn)的互斥力問題；第二，如何進(jìn)行邏輯控制，使機(jī)器人運(yùn)動(dòng)。

針對(duì)第一個(gè)問題，文獻(xiàn)[5]，文獻(xiàn)[6]提出的目前大多數(shù)樣機(jī)采用的方法得到的運(yùn)動(dòng)軌跡如圖2所示，即機(jī)器人爬行過程中，在跟關(guān)節(jié)處從俯視圖看，各條腿處于支撐態(tài)時(shí)走過的軌跡，當(dāng)機(jī)體質(zhì)量較大時(shí)，會(huì)對(duì)機(jī)體兩側(cè)腿部各關(guān)節(jié)的擠壓力導(dǎo)致關(guān)節(jié)損傷嚴(yán)重，因此根據(jù)實(shí)際應(yīng)用上的需求，在文獻(xiàn)[5],文獻(xiàn)[6]所提出的算法上進(jìn)行了改進(jìn)，在機(jī)器人行走過程中足端距機(jī)體位置變化進(jìn)行了約束，將這種約束產(chǎn)生的影響分布在每條腿的三個(gè)關(guān)節(jié)上，利用這三個(gè)關(guān)節(jié)協(xié)同作用，使得每條腿在處于支撐態(tài)時(shí)，足端走過一步的運(yùn)動(dòng)軌跡為一條直線，平行于前進(jìn)方向，即圖3所示，這樣分布于機(jī)體兩側(cè)的腿處于支撐態(tài)走過一步的軌跡相互平行，這樣就不存在互斥力。

圖2 改進(jìn)前的單腿支撐態(tài)走過的軌跡 圖3 改進(jìn)后單腿支撐態(tài)走過的軌跡

針對(duì)第二個(gè)問題，目前六足機(jī)器人爬行過程中有三足步態(tài)，四足步態(tài)和六足步態(tài)。相比于其他步態(tài)，三足步態(tài)具有控制相對(duì)簡單，爬行速度快的優(yōu)點(diǎn)，本文采用三足步態(tài)控制機(jī)器人運(yùn)動(dòng)。

根據(jù)機(jī)器人自身結(jié)構(gòu)特點(diǎn)，參考文獻(xiàn)[7],文獻(xiàn)[8]提出的D-H坐標(biāo)系的建立方法，建立機(jī)體坐標(biāo)系和單腿坐標(biāo)系。推導(dǎo)出正逆運(yùn)動(dòng)學(xué)方程，參考文獻(xiàn)[9]中對(duì)機(jī)器人的運(yùn)動(dòng)仿真方法，對(duì)機(jī)器人處于支撐態(tài)行走一步所需要的角度進(jìn)行仿真，給出仿真曲線。

1 單腿坐標(biāo)系的建立

本文設(shè)計(jì)的六足機(jī)器人共18個(gè)關(guān)節(jié)，每條腿上安裝3個(gè)關(guān)節(jié)，對(duì)每一條單步行腿，根據(jù)文獻(xiàn)[10],文獻(xiàn)[11],文獻(xiàn)[12],采用D-H法建立坐標(biāo)系，D-H法的具體規(guī)則描述如下：

1)確定zij軸，均垂直于軸向，各關(guān)節(jié)圍繞此軸轉(zhuǎn)動(dòng)；

2)確定xij軸，xij通過zij-1和zij的公法線方向，由zij-1指向zij；

3)確定yij軸，用右手定則判定。

其中i表示腿1～6中具體的某一條腿，j表示每條腿上的關(guān)節(jié)。依據(jù)此方法，對(duì)單腿進(jìn)行如下圖4所示的坐標(biāo)系建立：

圖4 單步行腿各關(guān)節(jié)坐標(biāo)系示意圖

圖4中O0，O1，O2分別表示跟關(guān)節(jié)，髖關(guān)節(jié)，膝關(guān)節(jié)的位置，O3的存在是為了便于分析足端位置，所以也在足端建立一個(gè)坐標(biāo)系。在O0，O1，O2處分別繞z0，z1，z2旋轉(zhuǎn)即形成θ1，θ2，θ3。

2 系統(tǒng)硬件設(shè)計(jì)

2.1 正運(yùn)動(dòng)學(xué)解

以機(jī)體中心為坐標(biāo)原點(diǎn)建立坐標(biāo)系，所有的腿部及關(guān)節(jié)坐標(biāo)均以機(jī)體坐標(biāo)系為參考，定義參數(shù)如下：

L0為身體中心到第一關(guān)節(jié)，即跟關(guān)節(jié)的距離；

θ0為身體中心到第一關(guān)節(jié)(跟關(guān)節(jié))連線與Yb的夾角，由機(jī)器人本身結(jié)構(gòu)參數(shù)決定；

θ1,θ2,θ3為機(jī)器人每條腿的第一關(guān)節(jié)角，第二關(guān)節(jié)角和第三關(guān)節(jié)角：

l1為第一關(guān)節(jié)(跟關(guān)節(jié))到第二關(guān)節(jié)，即髖關(guān)節(jié)的距離；

l2為第二關(guān)節(jié)(髖關(guān)節(jié))到第三關(guān)節(jié)，即膝關(guān)節(jié)的距離；

l3為第三關(guān)節(jié)(膝關(guān)節(jié))到腳底，即足端的距離。

根據(jù)文獻(xiàn)[13]，文獻(xiàn)[14]，文獻(xiàn)[5]，腿i跟關(guān)節(jié)坐標(biāo)系到髖關(guān)節(jié)坐標(biāo)系的旋轉(zhuǎn)矩陣i0Ri1，髖關(guān)節(jié)坐標(biāo)系到膝關(guān)節(jié)坐標(biāo)系i1Ri2，膝關(guān)節(jié)坐標(biāo)系到足端坐標(biāo)系的旋轉(zhuǎn)矩陣i2Ri3分別為：

(2)

(3)

可得腿i足端坐標(biāo)系原點(diǎn)在跟關(guān)節(jié)坐標(biāo)系中的旋轉(zhuǎn)矩陣為：

i0Ri3=i0Ri1*i1Ri2*i2Ri3=

(4)

進(jìn)而可以推導(dǎo)出機(jī)器人腿i跟關(guān)節(jié)，髖關(guān)節(jié)，膝關(guān)節(jié)坐標(biāo)系間的齊次變換矩陣如下：

(7)

由式(5)，(6)，(7)可得，跟關(guān)節(jié)到足端的變換矩陣為:

(8)

其中：

a11=cosθi1cos(θi1+θi2) ;

a12=-cosθi1sin(θi2+θi3) ;

a13=sinθi1;

a14=(l1+l2cosθi2+l3cos(θi2-θi3))cosθi1;

a21=sinθi1cos(θi2+θi3);

a22=-sinθi1sin(θi2+θi3);

a23=-cosθi1;

a24=(l1+l2cosθi2+l3cos(θi2-θi3))sinθi1;

a31=sin(θi2+θi3);

a32=cos(θi2+θi3);

a34=l2sinθi2+l3sin(θi2-θi3)。

根據(jù)(8)式可得足端坐標(biāo)原點(diǎn)在其跟關(guān)節(jié)坐標(biāo)系中的位置矢量為：

(9)

機(jī)體坐標(biāo)系XbOYb到跟關(guān)節(jié)的旋轉(zhuǎn)矩陣(繞z軸旋轉(zhuǎn))為：

(10)

其中:θ0由機(jī)體本身結(jié)構(gòu)所決定，d1繞y軸平移的距離，d2為繞x軸平移的距離，當(dāng)腿i確定時(shí)，對(duì)應(yīng)的參數(shù)即可確定。

各條腿i足端坐標(biāo)原點(diǎn)xp,yp,zp在其機(jī)體中心O處的坐標(biāo)為:

(11)

其中:

b1=d2+l1sin(θ0+θi1)+l2cosθi2sin(θ0+θi1)+l3sin(θ0+θi1)cos(θi2-θi3);

b2=d1+l1cos(θ0+θi1)+l2cosθi2cos(θ0+θi1)+l3cos(θ0+θi1)cos(θi2-θi3);

b3=l2sinθi2+l3sin(θi2-θi3);

其中:zp在這里被當(dāng)做標(biāo)量處理，只作數(shù)值，不考慮正負(fù)。

比如要確定腿2的足端位置時(shí)，做出其俯視圖，如圖5所示，從俯視圖看，XbOYb為機(jī)身坐標(biāo)，X0O0Y0表示腿2跟關(guān)節(jié)處坐標(biāo)。Z軸都垂直平面向上。由于每條腿具有相同的關(guān)節(jié)構(gòu)造，當(dāng)具體到某一條腿時(shí)，為了公式簡化，i的作用可以忽略。

圖5 坐標(biāo)旋轉(zhuǎn)示意圖

機(jī)體坐標(biāo)系向第二條腿跟關(guān)節(jié)旋轉(zhuǎn)平移，此時(shí)θ0=00，d1=L0，d2=0，所以:

(12)

(13)

其中:

c1=l1sinθ1+l2cosθ2sinθ1+l3sinθ1cos(θ2-θ3);c2=L0+l1cosθ1+l2cosθ2cosθ1+l3cosθ1cos(θ2-θ3);c3=l2sinθ2+l3sin(θ2-θ3);

2.2 機(jī)體逆運(yùn)動(dòng)學(xué)解

所謂逆運(yùn)動(dòng)學(xué)解，即由坐標(biāo)位置，反解出每條腿各θ角的大小。在求解機(jī)體逆運(yùn)動(dòng)學(xué)解時(shí)，采用幾何法進(jìn)行求解。

在圖6中，本文給出的是腿2的坐標(biāo)示意圖，(x,y)表示腿2足端在機(jī)體坐標(biāo)系中坐標(biāo)，機(jī)體在沿Xb軸前進(jìn)的過程中，D保持不變。

(14)

對(duì)于θ2，θ3，如圖7所示，由余弦定理:

(15)

在這里z的值為取絕對(duì)值之后的量，在腿實(shí)際行走且處于支撐相的過程中，z一定是一個(gè)負(fù)數(shù)，所以θ2的實(shí)際角度應(yīng)為:

(16)

(17)

(18)

θ3=π-θ4

(19)

式中,b的大小由行走時(shí)支撐腿足端與跟關(guān)節(jié)在前進(jìn)方向上所形成的兩條平行線間的距離D來決定，D的大小為y-d1，當(dāng)θ1變化時(shí)，bcosθ1=D。ZbOYb為機(jī)體坐標(biāo)系，l1為第一關(guān)節(jié)長度，大小可為0，(y,z)為腿足端在機(jī)體坐標(biāo)系下的坐標(biāo)。

圖6 機(jī)體俯視圖

圖7 關(guān)節(jié)平面圖

3 系統(tǒng)軟件設(shè)計(jì)

3.1 設(shè)計(jì)思路和編程方法

機(jī)器人行走的步態(tài)主要包括前進(jìn)/后退，左移/右移，左轉(zhuǎn)/右轉(zhuǎn)。在圖5中，①，②，③，④，⑤，⑥代表機(jī)器人的六條腿，為了便于描述將其依次命名為左前腿，左中腿，左后腿，右后腿，右中腿，右前腿。三足步態(tài)下①，③，⑤為一組，②，④，⑥為一組交替行走。下面具體說明機(jī)器人行走的控制策略。對(duì)前進(jìn)/后退，一共分為三個(gè)階段進(jìn)行控制，分別為初始化階段，循環(huán)階段和停止階段。

初始化階段：

1)①，③，⑤抬起，②，④，⑥不動(dòng)。這一步是為了讓①，③，⑤腿為下一步抬起后前進(jìn)/后退做準(zhǔn)備，②，④，⑥支撐機(jī)體保持穩(wěn)定。

2)①，③，⑤抬起后前進(jìn)/后退，②，④，⑥不動(dòng)。這一步是為了讓①，③，⑤腿處于目標(biāo)位置，便于下一步下落后機(jī)體做好前進(jìn)/后退準(zhǔn)備。②，④，⑥仍支撐機(jī)體保持穩(wěn)定。

3)①，③，⑤下落，②，④，⑥不動(dòng)。此時(shí)機(jī)器人六條腿都支撐在地面，①，③，⑤在目標(biāo)位置。

4)①，③，⑤保持不動(dòng)，②，④，⑥抬起。這一步是為了下一步機(jī)體前進(jìn)/后退做準(zhǔn)備。

5)①，③，⑤處于支撐態(tài)前進(jìn)/后退一步，②，④，⑥抬起后不動(dòng)。通過這一步，機(jī)體完成前進(jìn)/后退動(dòng)作。

6)①，③，⑤保持不動(dòng)，②，④，⑥抬起后前進(jìn)/后退。此舉是為了讓②，④，⑥腿達(dá)到目標(biāo)位置，為下一步前進(jìn)/后退移動(dòng)做好準(zhǔn)備。此時(shí)，①，③，⑤在前/后，②，④，⑥在后/前，因此這一步也是機(jī)械腿最有可能造成干涉的一步，如果此步?jīng)]有干涉，那么其他步驟也不可能造成干涉；如果此步有干涉，將前進(jìn)/后退一步的距離變小即可。

7)①，③，⑤保持不動(dòng)，②，④，⑥下落。在這一步中，②，④，⑥下落達(dá)到目標(biāo)位置，為下一步前進(jìn)/后退做好準(zhǔn)備。

8)①，③，⑤抬起，②，④，⑥保持不動(dòng)。此舉是為下一步前進(jìn)/后退做準(zhǔn)備。

9)①，③，⑤抬起保持不動(dòng)，②，④，⑥處于支撐態(tài)前進(jìn)/后退一步。通過這一步，機(jī)器人達(dá)到運(yùn)動(dòng)目的。

以上9步即為初始化階段過程，機(jī)器人走過這個(gè)階段，進(jìn)入循環(huán)階段。

循環(huán)階段：在這一階段，機(jī)器人通過不斷循環(huán)走這一步，可以達(dá)到前進(jìn)/后退的目的，循環(huán)階段又分為以下8個(gè)步驟。

1)①，③，⑤抬起后前進(jìn)/后退，②，④，⑥保持不動(dòng)。這一步是讓①，③，⑤在下一步下落后達(dá)到目標(biāo)位置，為下一步動(dòng)作做好準(zhǔn)備。

2)①，③，⑤下落著地，②，④，⑥保持不動(dòng)。此時(shí)①，③，⑤到達(dá)目標(biāo)位置。

3)①，③，⑤不動(dòng)，②，④，⑥抬起。此時(shí)機(jī)體由①，③，⑤腿支撐，為下一步動(dòng)作做好準(zhǔn)備。

4)①，③，⑤處于支撐態(tài)前進(jìn)/后退一步，②，④，⑥不動(dòng)，通過這一步，機(jī)器人達(dá)到前進(jìn)/后退效果。

5)①，③，⑤處于支撐態(tài)不動(dòng)，②，④，⑥抬起后前進(jìn)/后退一步。這一步是為了讓②，④，⑥腿處于目標(biāo)位置，便于下一步下落后機(jī)體做好前進(jìn)/后退準(zhǔn)備。

6)①，③，⑤不動(dòng)，②，④，⑥下落，此時(shí)②，④，⑥在目標(biāo)位置就緒。

7)①，③，⑤抬起，②，④，⑥保持不動(dòng)。此時(shí)機(jī)體由②，④，⑥腿支撐，且②，④，⑥腿處于目標(biāo)位置。

8)①，③，⑤抬起后保持不動(dòng)，②，④，⑥處于支撐態(tài)前進(jìn)/后退一步。通過這一步，機(jī)體完成前進(jìn)/后退動(dòng)作。

可以發(fā)現(xiàn)，在循環(huán)階段第8步的結(jié)束狀態(tài)即為第1步開始前的狀態(tài)，因此通過程序中設(shè)置循環(huán)的次數(shù)即可控制機(jī)器人前進(jìn)步數(shù)。

停止階段：即在循環(huán)中，接到停止命令，使機(jī)器人回到最初狀態(tài)，為后續(xù)左右移動(dòng)，轉(zhuǎn)彎做好準(zhǔn)備。該階段的控制策略與初始化階段步驟相似，只需要按照與初始化階段相反的執(zhí)行動(dòng)作即可。

對(duì)于左移/右移，左轉(zhuǎn)/右轉(zhuǎn)的控制策略同前進(jìn)/后退,按照機(jī)器人實(shí)際運(yùn)動(dòng)方式進(jìn)行控制即可，在此不再贅述。

編程時(shí)，以足端坐標(biāo)的變化范圍為控制變量，將行走過程分解為多步，每一步對(duì)應(yīng)相應(yīng)的坐標(biāo)變化，使得程序能夠按順序執(zhí)行下去。以其中一步為例說明：

while((z1>10)&&(z3>10)&&(z5>10))

{

semTake(semIdForward,WAIT_FOREVER);//創(chuàng)建信號(hào)量

formula(y1,z1,y2,z2,y3,z3,y4,z4,y5,z5,y6,z6);//調(diào)用算法函數(shù)，y1,z1表示第一條腿足端坐標(biāo)，以此類推

fuzhi();//解算關(guān)節(jié)值賦給結(jié)構(gòu)體

sendNum=send (clientSock_duozhou, (char*)&crawl, sizeof(crawl), 0);//網(wǎng)絡(luò)發(fā)送

DataRecv_duozhou();//接收電機(jī)返回值

z1--,z3--,z5--;//坐標(biāo)變化

}

3.2 軟件實(shí)現(xiàn)流程圖

本文以前進(jìn)/后退為例，說明三足步態(tài)下機(jī)器人的控制方式，具體流程如圖8所示。

圖8 三足步態(tài)控制策略

4 實(shí)驗(yàn)結(jié)果與分析

本文對(duì)正逆運(yùn)動(dòng)學(xué)算法采用Matlab仿真，利用其中的Simulink模塊，將正逆算法公式進(jìn)行搭建，輸入為各腿在機(jī)體坐標(biāo)系下的坐標(biāo)，輸出為對(duì)應(yīng)坐標(biāo)下各關(guān)節(jié)角度，具體步驟為：

1)確定各腿在機(jī)體坐標(biāo)系下x,y,z坐標(biāo)，該坐標(biāo)的確定方法為：根據(jù)腿部各關(guān)節(jié)長度大小，以及期望各關(guān)節(jié)中心角度，利用正運(yùn)動(dòng)學(xué)解公式，大概解算出各坐標(biāo)大小，在該坐標(biāo)附近確定一個(gè)值，通過逆運(yùn)動(dòng)學(xué)仿真得到該腿行走一步所經(jīng)過的抬起，平移，落地，支撐推進(jìn)階段所需要的關(guān)節(jié)角度為平滑曲線，則該坐標(biāo)可以作為機(jī)器人動(dòng)作點(diǎn)。

2)在機(jī)器人行走時(shí)，某些點(diǎn)在其行走過程中可以作為不動(dòng)點(diǎn)，比如前進(jìn)/后退時(shí)，各腿y坐標(biāo)保持不動(dòng)；左移/右移時(shí)，x坐標(biāo)保持不變。仿真時(shí)，變化的坐標(biāo)可以通過系統(tǒng)按時(shí)間生成，所得角度即為機(jī)器人運(yùn)動(dòng)控制所需的角度。

3)機(jī)器人運(yùn)動(dòng)時(shí)點(diǎn)的選取原則為理論和實(shí)驗(yàn)結(jié)合，保證機(jī)器人運(yùn)動(dòng)所需的角度合理即可，同時(shí)要滿足支撐態(tài)時(shí)足端距跟關(guān)節(jié)在水平面的投影大于設(shè)定的平行間距。

4)步長的大小要根據(jù)機(jī)器人尺寸來確定，即d2的大小，若步長太大，則會(huì)超出機(jī)器人運(yùn)動(dòng)擺幅極限，左前腿向后擺動(dòng)，左中腿向前擺動(dòng)時(shí)，產(chǎn)生物理干涉，機(jī)器人無法運(yùn)動(dòng)；若步長太小，則機(jī)器人行走速度慢，電機(jī)做了大量的無用功，理論上來說，機(jī)器人支撐所走的一個(gè)步長不能超過d2的大小，考慮到機(jī)械腿有一定的寬度，實(shí)際上來說機(jī)器人支撐所走的一個(gè)步長不能超過0.8*d2的大小。平行間距的確定：前進(jìn)/后退時(shí)，若各腿足端坐標(biāo)確定，間距的大小等于對(duì)應(yīng)y坐標(biāo)值減去跟關(guān)節(jié)距機(jī)體坐標(biāo)系y軸的值。

基于此，本文給出腿2處于支撐態(tài)前進(jìn)一步的角度仿真曲線，設(shè)定機(jī)器人支撐行走時(shí)一個(gè)步長為14 cm，所以腿2足端坐標(biāo)xp2在機(jī)體坐標(biāo)系下的變化范圍可以是7 cm至-7 cm(坐標(biāo)變化起止的范圍可以不同，但為了便于觀察機(jī)器人走的步長以及直觀看出逆運(yùn)動(dòng)學(xué)解的對(duì)稱性，所以建議以各腿y軸為中心進(jìn)行擺動(dòng))，給定機(jī)體坐標(biāo)系下yp2=22 cm(由于D的存在，走過一步軌跡平行于前進(jìn)方向，所以yp2為一個(gè)定值)，zp2=-10 cm。得到θ1，θ2，θ3的仿真曲線如圖9所示。

圖9 θ1，θ2，θ3隨xp2在機(jī)體坐標(biāo)系中的變化情況

實(shí)驗(yàn)發(fā)現(xiàn)Simulink解算出的角度值與理論分析的一致，將角度再帶入正運(yùn)動(dòng)學(xué)方程，得到值即為設(shè)定的坐標(biāo)值。將此算法通過c語言編程控制電機(jī)運(yùn)動(dòng)，機(jī)器人可以實(shí)現(xiàn)相應(yīng)的步態(tài)行走，在前進(jìn)/后退實(shí)驗(yàn)室時(shí)，機(jī)器人兩側(cè)腿處于支撐態(tài)走出的軌跡曲線大致相互平行。

5 結(jié)論

本文建立了機(jī)器人的運(yùn)動(dòng)學(xué)模型，在原有基礎(chǔ)上對(duì)腿部各角度運(yùn)動(dòng)控制算法進(jìn)行了改進(jìn)，給出了解算機(jī)器人空間位置坐標(biāo)下所需要的各關(guān)節(jié)角度的方法，即機(jī)體逆運(yùn)動(dòng)學(xué)解；以及給出各關(guān)節(jié)角度下，足端所能達(dá)到的空間位置，即機(jī)體正運(yùn)動(dòng)學(xué)解。通過Simulink仿真實(shí)驗(yàn)和c語言編程控制機(jī)器人運(yùn)動(dòng)，證實(shí)了理論的正確性。