專利名稱：多關節(jié)仿生機器魚的運動控制方法
技術領域：
本發(fā)明涉及多關節(jié)仿生機器魚的運動控制方法。
背景技術：
作為生物機理和工程技術的結合點，仿生機器魚已成為一項包含水動力學控制和驅動技術的多學科研究。由于仿生機器魚在水中的運動涉及流體環(huán)境的水動力學和魚體的運動學，在現有水動力學分析的基礎上還無法建立起機器魚游動的復雜水動力學模型，因此很難通過解析的方法建立一個精確的水動力學推進模型來指導機器魚的設計與控制。魚類行為學家的研究表明，魚類的推進運動中隱含著一由后頸部向尾部傳播的行波。受此啟發(fā)，人們嘗試從運動學的角度來研究魚類的推進，以避免復雜的水動力學分析。1996年，美國MIT的Barrett等人提出了一個金槍魚(典型的鲹科魚類)的推進模型并推導了決定金槍魚游動效率的七個獨立的主要參數。如何合理選取這7個獨立的參數對于不同種類、外形和尺寸的魚來說，仍舊是一個非常棘手的問題。當采用逐一實驗的辦法時，假設每個參數的取值范圍對應10個值，實驗空間的元素數目高達710＝282475249。所以，基于簡單的常態(tài)游動的運動學模型，研究一種參數數目少、便于控制、易于實現的機器魚游動控制方法是機器魚理論設計和運動控制的關鍵性技術問題。

發(fā)明內容
本發(fā)明的目的是提供一種基于關節(jié)結構的仿鲹科模式推進的機器魚的運動控制方法，避免了復雜的水動力學分析，可實現比較精確的運動控制。
為實現上述目的，一種多關節(jié)仿生機器魚的運動控制方法，包括步驟
通過調節(jié)關節(jié)的擺動頻率控制機器魚直行游動的速度；通過施加不同的關節(jié)偏移控制機器魚的方向。
本發(fā)明將為多關節(jié)仿生機器魚的理論設計提供參考并實現機動、靈活的游動控制，可實際應用于水下航行器的設計、水下資源的勘測、水下救撈、水下設施的維護與巡檢以及海洋領土的防御等方面。


圖1是基于關節(jié)結構的魚體波曲線擬合圖；圖2是機器魚的機械結構圖。
具體實施例方式
魚類的推進運動中隱含著一由后頸部向尾部傳播的行波，該推進波主要表現脊柱和肌肉組織的彎曲，其幅度由前向后逐漸增加，其傳播速度大于魚體的前進速度。鲹科魚類的魚體波曲線可看作是魚體波幅包絡線和正弦曲線的合成，它開始于魚體的慣性力的中心，延伸至尾柄，其曲線方程可表示為ybody(x，t)＝[(c1x+c2x2)][sin(kx+ωt)](1)這里，ybody是魚體的橫向位移，x是魚體的軸向位移，k是體波數(k＝2π/λ)，λ是魚體波的波長，c1是魚體波波幅包絡線的一次項系數，c2是魚體波波幅包絡線的二次項系數，ω是魚體波頻率(ω＝2πf＝2π/T)。
根據鲹科魚類的這種波動推進原理，可以通過擬合(1)式中的魚體波曲線來設計多關節(jié)的仿生機器魚，而本發(fā)明主要針對用N(N≥2)個關節(jié)來擬合魚體波曲線運動、仿鲹科模式機器魚的游動控制問題。仿生機器魚的關節(jié)由電機直接驅動或通過鋼絲繩、拉桿等傳動元件帶動。用本發(fā)明所提供的二維擺動數組作為其游動數據，可以實現機器魚在水中的游動控制。
由于計算機控制是一種數字量控制，在本發(fā)明中我們考慮將魚體波曲線離散化，把時間參數t從魚體波運動函數ybody(x，t)中分離出來。也就是說，將不斷傳播的魚體波分解成兩部分1)一個擺動周期內，與時間無關的樣條曲線序列(見式(2))ybody(x，i)(i＝0，1，...，M-1)；2)與時間相關的擺動頻率f，即單位時間內擺動機構完成體波運動的次數。
ybody(x,i)=[(c1x+c2x2)][sin(kx-2πMi)]----(2)]]>這里，i是一個擺動周期內的樣條曲線序列變量；M是魚體波分辨率，即在一個擺動周期內整個魚體波被離散的程度，其上限為伺服電機的最高擺動頻率。
在擬合魚體波曲線之前，我們定義魚體波相對波長R(0＜R≤1)為魚體擺動部分的長度占整個正弦波的比率。當R接近0時，魚體擺動部分可視為一剛性棒，很難產生推進力；當R接近1時，魚體擺動部分呈現一個完整的正弦波；當R取得合適的值時，魚體在水中的運動比較平穩(wěn)、高效。根據理論分析和實驗所獲得的經驗知識，本發(fā)明主要是對采用直流伺服電機驅動、魚體相對波長在0.3-0.6左右的機器魚運動控制時使用本發(fā)明提供的擺動數據。
如圖1所示，魚體在擺動平面的運動(在0到Rπ2的軸向區(qū)間)可用一系列(N個)串聯(lián)的連桿機構來表示。假定每根桿的長度分別為l1，l2...，lN(這里，li長度未知，但l1，l2...，lN之間的長度比可事先設定，不妨設l1∶l2∶...，∶lN＝l1′∶l2′∶...，∶lN′)，其對應的關節(jié)角為φ1，φ2，...，φN，其對應的端點坐標為(x0，y0)，(x1，y1)，...，(xN，yN)，機器魚機動性的控制主要依靠內在形狀(關節(jié)角φj，1≤j≤N)的改變，其速度的控制依靠擺動頻率f的改變。一旦選定魚體波波幅包絡線系數(c1，c2)和體波數k(k＝1)，某一時刻的魚體波形狀就大體上確定了。
從數學意義上來說，對于一個擺動周期內不同時刻(i)的關節(jié)角φij可通過擬合魚體波曲線來計算。所要解決的問題是在0到Rπ2的軸向區(qū)間內尋找合適的關節(jié)角φij使lj在魚體波曲線上首尾相接、并且最后一根桿末端點的橫坐標剛好落在Rπ2處。即滿足下列條件(見式(3)) 式中，(xi，j，yi，j)為一個擺動周期內第i時刻第j根桿的端點坐標，xi，0＝0，xi，N＝R×2π，1≤j≤N，0≤i≤M-1。
對于方程組(3)來說，由于包含xi，j的平方項，且lj未知，當N增大時，很難通過解析的方法計算方程組的精確解(xi，j，yi，j)。并且實際運算中總會引入舍入誤差，即使采用精確法求得的解也只能是近似解。在這種情況下，本發(fā)明采用迭代算法，它通過選取某種初值，用同樣的計算步驟，反復計算，求得方程組的近似解。其具體步驟如下(a)迭代逼近桿長系數m對等式(xi，j-xi，j-1)2+(yi，j-yi，j-1)2＝lj2進行移項，構造誤差函數f(xi，j-1，yi，j-1，xi，j，yi，j，m)＝(xi，j-xi，j-1)2+(yi，j-yi，j-1)2-(m×lj′)2(這里m為桿長系數)，選擇m的初值(m＝0.0)，從j＝1、i＝0開始迭代計算，直到滿足f(xi，j-1，yi，j-1，xi，j，yi，j，m)＜LError(LError為設定的桿長誤差)，接著j＝j+1，轉入下一根桿的迭代計算，直到計算第N根桿，判別xN是否等于R×2π，即計算|xN-R×2π|＜XError是否成立。如果該絕對誤差表達式成立，此時m值即為所求的桿長系數，循環(huán)結束。否則，m＝m+STEP(STEP為迭代步長，這里取STEP＝0.01)，轉入下一趟循環(huán)，進行從第1根桿到第N根桿的迭代計算，直到滿足循環(huán)結束條件|xN-R×2π|＜XError。
(b)計算關節(jié)坐標對(xij，yij)記錄循環(huán)結束時的桿長系數m，由于lj＝m×lj′將lj的近似值回代方程組(3)，解方程組，依次可求出坐標對(x01，y01)，(x02，y02)，...，(x0N，y0N)。i＝i+1，轉入下一時刻(i+1)的坐標對(x11，y11)，(x12，y12)，...，(x1N，y1N)的求解。反復計算，直到計算出i＝M-1時刻的坐標對(xM-1，1，yM-1，1)，(xM-1，2，yM-1，2)，...，(xM-1，N，yM-1，N)。
(c)計算桿(lj)與魚體主軸(x軸)的夾角γij根據上一步中求出的擺動周期內任意時刻i、第j根桿的端點坐標(xij，yij)，利用公式(4)可求出該桿(lj)與魚體主軸(x軸)的夾角γij(見式(4))γi，j＝a tan 2((xi，j-xi，j-1)，(yi，j-yi，j-1))×180°/π (4)(d)計算二維關節(jié)擺動數組OscData[M][N]利用上一步求出的桿與魚體縱軸的夾角γij，可求出擺動周期內任意時刻i、第j-1根桿與第j根桿的夾角φij(見式(5))φi，j＝γij-γi，j-1(5)
這里，假定γi，0＝0°。
從而，可以得到一個M×N的二維關節(jié)擺動數組OscData[M][N](見式(6))OscData[M][N]=φ01φ02···φ0Nφ11φ12···φ1N············φM-1,1φM-1,2···φM-1,N----(6)]]>利用式(6)得出的二維關節(jié)擺動數組作為機器魚的游動數據，通過施加不同的關節(jié)偏移，可以調整機器魚擺動部分的扭曲形狀，從而實現不同的方向控制。同時，在游動過程中，通過改變擺動頻率f可以改變機器魚在水中的推進力，進而實現不同的速度控制。由于這些運動控制參數都與魚體的外形和尺寸無關，故僅僅依靠參數集{φ1，φ2，...，φN，f}就可以實現機器魚在水中的控制。
在機器魚游動過程中需要對方向加以控制時，采用對從關節(jié)1到關節(jié)K的K個串行連接關節(jié)的擺動數據增加固定偏轉角度的方法，其中關節(jié)數目K小于N/2。當機器魚在(6)式的擺動數據控制下運動時，若機器魚需要轉向α度的方向時，(6)式的擺動數據將調整為OscData[M][N]=φ01+αφ02+α···φ0Nφ11+αφ12+α···φ1N············φM-1,1+αφM-1,2+α···φM-1,N----(7)]]>其中添加α度偏移角的關節(jié)為Φ1-ΦN/2之間的從Φ1開始的連續(xù)K個關節(jié)，K值可依據具體需求確定，但不得大于N/2。方向偏轉角度α以魚體前部的中心線為基準，其為正時機器魚逆時針轉|α|，其為負時機器魚順時針轉|α|，α的最大值受所設計的機器魚機構和驅動電機的特性限制。當α為0度時，(7)簡化為(6)式。機器魚采用(7)式進行控制時就可以在水中實現轉向運動。
實施例本發(fā)明提供的多關節(jié)仿生機器魚的運動控制方法應用于中國科學院自動化研究所復雜系統(tǒng)與智能科學實驗室研制的四關節(jié)機器魚，見圖2。該機器魚的關節(jié)采用直流伺服電機驅動，具體的技術參數見表1。
表1機器魚的技術參數

OscData[18][4]=4.07-3.286.8512.682.46-5.843.6512.640.55-7.730.0610.87-1.43-8.70-3.477.66-3.25-8.63-6.573.59-4.66-7.50-8.94-0.744-5.51-5.44-10.30-4.85-5.69-2.71-10.41-8.43-5.200.31-9.23-11.16-4.073.28-6.85-12.68-2.465.85-3.65-12.64-0.557.73-0.06-10.871.438.703.47-7.663.258.636.57-3.594.667.508.940.745.515.4410.294.855.692.7110.428.435.20-0.319.2311.16----(8)]]>機器魚的擺動控制數據集成在頭部的微控器內。通過微控器控制CPLD，CPLD經調制產生PWM信號，控制各個直流伺服電機的轉角，從而實現各關節(jié)的協(xié)調擺動。在機器魚的一個擺動周期內，魚體波擬合的參數如下c1＝0.1，c2＝0，k＝1，R＝0.5，M＝18，N＝4，l1′∶l2′∶l3′∶l4′＝1∶1∶1∶1。該推進模型最終的結果是一個18×4的關節(jié)擺動數組OscData[18][4](見式(8)，單位度)和擺動頻率f。
在機器魚的速度控制實驗中，通過改變伺服電機的擺動頻率f，得到機器魚的直行游動速度和頻率的關系(見表2)游動速度隨著擺動頻率的增加面增加，但最大的游動速度受伺服電機的最大擺動頻率所限制。
表2機器魚的電機的擺動頻率(Hz)和直行游動速度(m/s)的關系

在機器魚的方向控制實驗中，測試讓機器魚以不同的轉彎半徑完成一個2π的圓周運動。這里，假定所有關節(jié)角φi(i＝1，2，3，4)的中間位置(平行于體軸)是0°，左旋轉極限是-30°，右旋轉極限是30°。如果將電機整個轉動范圍分成8檔(-4，-3，-2，-1檔為左轉，1，2，3，4檔為右轉)，每檔代表7.5°。當選擇不同的轉彎檔(L)，在每個擺動周期內，相應的關節(jié)角偏移量(L×7.5°)迭加到前兩個關節(jié)角(φ1和φ2)，從而魚體彎向一側，實現不同半徑的轉彎。相關的實驗數據見表3。
表3機器魚在擺動頻率為2Hz時的角速度(rad/s)

權利要求
1.一種多關節(jié)仿生機器魚的運動控制方法，包括步驟通過調節(jié)關節(jié)的擺動頻率控制機器魚直行游動的速度；通過施加不同的關節(jié)偏移控制機器魚的方向。
2.按權利要求1所述的方法，其特征在于所述機器魚擺動包括步驟迭代逼近桿長系數m；計算關節(jié)坐標對(xij，yij)；計算桿(li)與魚體主軸(x軸)的夾角γij；計算二維關節(jié)擺動數組OscData[M][N]。
3.按權利要求1所述的方法，其特征在于所述機器魚的關節(jié)為多個串聯(lián)連接的關節(jié)。
4.按權利要求1所述的方法，其特征在于所述方向控制包括步驟對從關節(jié)1到關節(jié)K的K個串行連接關節(jié)的擺動數據增加固定偏轉角度，其中關節(jié)數目K小于N/2。
5.按權利要求1所述的方法，其特征在于所述速度控制和方向控制分離。
全文摘要
一種多關節(jié)仿生機器魚的運動控制方法，包括步驟通過調節(jié)關節(jié)的擺動頻率控制機器魚直行游動的速度；通過施加不同的關節(jié)偏移控制機器魚的方向。本發(fā)明將為多關節(jié)仿生機器魚的理論設計提供參考并實現機動、靈活的游動控制，可實際應用于水下航行器的設計、水下資源的勘測、水下救撈、水下設施的維護與巡檢以及海洋領土的防御等方面。
文檔編號G05B15/00GK1595312SQ0315676
公開日2005年3月16日 申請日期2003年9月10日 優(yōu)先權日2003年9月10日
發(fā)明者王碩, 喻俊志, 張志剛, 桑海泉, 譚民 申請人:中國科學院自動化研究所