基于Kane動力學(xué)和通路矩陣的水下自重構(gòu)機器人建模

徐 鉆，楊 柯，葛 彤，王旭陽

（1.杭州電子科技大學(xué) 機械工程學(xué)院，杭州 310018；2.上海交通大學(xué) 船舶海洋與建筑工程學(xué)院，上海 200240）

0 引 言

水下自重構(gòu)機器人由相同模塊組成，可以根據(jù)環(huán)境和任務(wù)需要通過自重構(gòu)策略動態(tài)重構(gòu)出所需的構(gòu)形。水下自重構(gòu)機器人構(gòu)形的變化增強了其在復(fù)雜多變的水下環(huán)境中的適應(yīng)能力[1-2]，但同時增加了建模的難度。與常規(guī)水下機器人AUV和ROV相比，水下自重構(gòu)機器人的運動學(xué)和動力學(xué)顯然要復(fù)雜很多[3-4]。

水下自重構(gòu)機器人建模需解決兩個問題：統(tǒng)一構(gòu)形描述方法和建模方法。統(tǒng)一構(gòu)形描述是指對水下自重構(gòu)機器人模塊間的運動關(guān)系進行描述。由于水下自重構(gòu)機器人的構(gòu)形是不斷變化的，因此水下自重構(gòu)機器人的統(tǒng)一構(gòu)形描述是比較困難的。為了解決這一問題，引入了圖論的相關(guān)知識。通過頂點和有向弧表征模塊的桿件和關(guān)節(jié)，建立運動關(guān)系圖，解決了水下自重構(gòu)機器人的表示方法問題。通過可達矩陣和通路矩陣描述模塊間的連接問題，這就解決了水下自重構(gòu)機器人的統(tǒng)一構(gòu)形描述問題。建模的方法比較多，主要包括 Newton-Euler方程[5-6]、Lagrange 方程[7]、Routh 方程、Appell方程[8]、Kane 方程[9-12]等。我們使用不同的方法對一個物體的同一種運動進行研究，最終得到的結(jié)果是相同的，但這些方法的計算速度和計算量是不同的。

Newton-Euler方程包含關(guān)節(jié)間的相互作用力，在推導(dǎo)機器人運動與力之間的關(guān)系時，相互作用力很難消除。因此，Newton-Euler方程主要用于設(shè)計階段對施加到機器人上的力進行分析。而Lagrange方程、Routh方程和Appell方程通常為非線性微分方程，對這些方程進行求解時運算速度慢、耗費時間長。Kane方程在解決復(fù)雜系統(tǒng)的動力學(xué)問題時非常方便。Kane方程比較直觀，因為它消除了Newton-Euler方程中存在的相互作用力和Lagrange方程需要的勢能函數(shù)。Kane方程使用廣義坐標，從而可以方便地施加約束。在Kane方法中使用廣義速度，使得為非完整約束系統(tǒng)選擇一組相互獨立的運動變量成為可能，從而獲得一階微分形式的動力學(xué)方程。Kane方程提供了一個將外部作用力添加到機器人模型中的直觀方法，并且Kane方程可以很容易地整理成非常適用于控制算法的閉環(huán)形式。最終的動力學(xué)模型可以為“哪些力真正影響動力學(xué)模型”提供物理解釋。

本文主要是針對水下自重構(gòu)機器人構(gòu)形多變的特點，探討了在動力學(xué)建模過程中存在的問題及解決方法，并提出了采用統(tǒng)一構(gòu)形描述與Kane方程相結(jié)合的方法解決水下自重構(gòu)機器人的動力學(xué)建模問題。

1 水動力計算

通常，作用在剛體上的水動力包括：浮力、拖曳力和拖曳力矩、附加質(zhì)量力和附加質(zhì)量矩[13]。水下自重構(gòu)機器人是由相同模塊組成（如圖1所示，、s和表示與模塊sk固結(jié)的相互垂直的單位矢量，滿足右手法則），每個模塊可以近似為長方體，因此，我們可以計算單個模塊的水動力，將所有模塊的水動力相加即可獲得水下自重構(gòu)機器人的水動力。

第k個模塊sk受到的浮力為

式中：ρ表示流體的密度；Vsk表示模塊sk排開流體的體積；g=[0 0 9.8 ]T為重力加速度；n為組成水下自重構(gòu)機器人的模塊數(shù)量。

模塊s的拖曳力可以表示為[3-4]

k

圖1 水下自重構(gòu)機器人的組成模塊Fig.1 The module of the underwater self-reconfigurable robot

模塊sk的拖曳力矩可以表示為

作用在模塊s上的附加質(zhì)量力和附加質(zhì)量矩為[3-4]

k

2 統(tǒng)一構(gòu)形描述

水下自重構(gòu)機器人可以根據(jù)任務(wù)需求和海洋環(huán)境信息動態(tài)重構(gòu)出不同構(gòu)形，因此，很難對水下自重構(gòu)機器人的所有構(gòu)形進行統(tǒng)一的描述。為了描述水下自重構(gòu)機器人的所有構(gòu)形，我們需要解決以下問題：對模塊的設(shè)計要求、運動關(guān)系描述、可達矩陣和通路矩陣。

（1）對模塊的設(shè)計要求。為了不失一般性，組成水下自重構(gòu)機器人的模塊應(yīng)滿足以下要求：①關(guān)節(jié)具有一個自由度；②桿件具有對稱的幾何特性；③桿件上具有多個連接端口。關(guān)節(jié)模塊可以通過標準的接口連接到桿件上。對稱的幾何特性可以使模塊具有靈活的裝配模式。在這種情況下，多自由度的運動可以由多個1自由度的關(guān)節(jié)組合來完成。

（2）運動關(guān)系描述。在機構(gòu)設(shè)計理論中，經(jīng)常采用圖論中的方法進行機構(gòu)的運動關(guān)系描述，即用頂點和有向弧表示機構(gòu)的桿件和關(guān)節(jié)[14]。使用這種圖形表示方法，我們可以對連桿機構(gòu)的基本結(jié)構(gòu)進行分類，并運用圖論中的理論對連桿機構(gòu)進行枚舉和分類。本文采用相同的符號表示水下自重構(gòu)機器人的基本組成模塊。圖2給出了9模塊8關(guān)節(jié)水下自重構(gòu)機器人的構(gòu)形圖和運動關(guān)系描述。

（3）可達矩陣和通路矩陣。為了自動產(chǎn)生模塊化機器人的動力學(xué)方程，我們引入了兩個矩陣：可達矩陣和通路矩陣。運動關(guān)系圖中兩個頂點（桿件）間的連接可以通過可達矩陣來確定。通過該矩陣，我們可以獲得模塊化機器人從基準模塊到其它任意模塊的可達信息。

圖2 水下自重構(gòu)機器人構(gòu)形和運動關(guān)系圖Fig.2 Configuration and kinematic relation graph of the underwater self-reconfigurable robot

定義1可達矩陣（Accessibility matrix）：用于描述n+1個模塊組成的水下自重構(gòu)機器人的運動關(guān)系的可達矩陣A是 （n+ 1 ）×（n+ 1 ）的矩陣，Aij（i，j=0，1，…，n ）是可達矩陣的元素。 如果存在一條由si到 sj的有向路徑，則 Aij=1；否則，Aij=0。

由于從si到si不存在有向路徑，可達矩陣A的所有對角線元素為0。例如，圖2所描述的水下自重構(gòu)機器人構(gòu)形的可達矩陣為：

從模塊化機器人的可達矩陣A中，我們可以尋找到從基點到某一桿件的唯一路徑，我們稱之為通路。如果我們僅考慮由基點到最末端桿件的通路，水下自重構(gòu)機器人構(gòu)形的通路數(shù)量等于最末端桿件的數(shù)量。實際上，最末端桿件對應(yīng)的是可達矩陣全為0的行。假設(shè)桿件si為最末端桿件，矩陣（A+I（n+1）×（n+1））T的第i+1行的所有非0元素表示的是從基點到si的通路上的桿件。找到所有的路徑，我們可以獲得下述矩陣：

定義2通路矩陣（Path matrix）：用于描述n+1個桿件和m條通路組成的水下自重構(gòu)機器人的運動關(guān)系的通路矩陣P是m×（n+ 1 ）矩陣，Pij（i=1，2，…，m；j=0，1，…，n）是通路矩陣的組成元素。如果通路 i包含頂點 j，則 Pij=1；否則，Pij=0。

描述水下自重構(gòu)機器人構(gòu)形的運動關(guān)系所需的信息（例如，通路數(shù)量、通路中模塊的連接序列、末端模塊等）可以清晰地在它的通路矩陣中表示出來。例如，圖2包含6條通路。6條通路可以表示為6×9的通路矩陣：

第一行表示水下自重構(gòu)機器人構(gòu)形包含模塊s0和s1的分支；第二行表示水下自重構(gòu)機器人構(gòu)形包含s0和s2的分支；第三行表示水下自重構(gòu)機器人構(gòu)形包含模塊s0、s3和s4的分支；第四行表示水下自重構(gòu)機器人構(gòu)形包含模塊s0、s3和s5的分支；第五行表示水下自重構(gòu)機器人構(gòu)形包含模塊s0、s6和s7的分支；第六行表示水下自重構(gòu)機器人構(gòu)形包含模塊s0、s6和s8的分支。比較可達矩陣和通路矩陣，可以看出通路矩陣P的行對應(yīng)的是A+I（n+1）×（n+1）中的第 2、3、5、6、8 和 9 列。

利用圖論中的圖形搜索技術(shù)和機構(gòu)的運動關(guān)系描述，我們可以用統(tǒng)一的數(shù)學(xué)公式來描述水下自重構(gòu)機器人的所有構(gòu)形。

3 水下自重構(gòu)機器人Kane動力學(xué)建模

3.1 運動學(xué)分析

水下自重構(gòu)機器人的坐標系及矢徑定義如圖3所示，所有坐標系的坐標軸均滿足右手法則。其中，sk表示水下自重構(gòu)機器人的第k個組成模塊；ck表示模塊sk的質(zhì)心；模塊sk相對于模塊 sj的轉(zhuǎn)動角度為，模塊 s0相對于慣性坐標系O的姿態(tài)角定義為和是與模塊sk固結(jié)的相互垂直的單位矢量，載體坐標系的原點與sk的起點skO重合，且坐標軸的延長線過模塊sk的質(zhì)心 ck；表示慣性坐標系O的原點到模塊sk的質(zhì)心ck的矢徑；表示慣性坐標系O的原點到模塊si的起點siO的矢徑。

圖3 水下自重構(gòu)機器人坐標系及矢徑定義Fig.3 Coordinate system and radius vector of the underwater self-reconfigurable robot

為了計算模塊sk相對于慣性坐標系O的姿態(tài)角，首先，需要確定模塊sk位于第幾條通路，即通路矩陣的第幾行；其次，需要記錄該行中從模塊s0到模塊sk的所有非零元素的編號。模塊sk相對于慣性坐標系O的姿態(tài)角可以表示為

式中：i和 j取模塊 s0到模塊 sk的通路中的模塊編號 （0≤i＜k，i＜j≤k ）；表示模塊 sj相對于模塊 si的轉(zhuǎn)動角度；如果 si和 sj位于模塊 s0到模塊 sk的通路中，且si和 sj相鄰，則 αij=1；否則，αij=0。同理，模塊sk相對于慣性坐標系O的角加速度為

模塊sk相對于慣性坐標系O的角加速度為

模塊sk的質(zhì)心相對于慣性坐標系O的矢徑可寫成：

公式（9）可重寫為

模塊sk的質(zhì)心相對于慣性坐標系O的速度可寫成

模塊sk的質(zhì)心相對于慣性坐標系O的加速度可寫成

偏速度和偏角速度為

3.2 動力學(xué)分析

模塊sk的慣性力和慣性矩為

式中：mk為模塊sk的質(zhì)量；為模塊sk的中心慣性張量。

模塊sk的廣義慣性力為

模塊sk受到的重力Gsk對廣義主動力的貢獻為

作用在模塊sk上的浮力Bsk對廣義主動力的貢獻為

作用在模塊sk上的拖曳力和拖曳力矩對廣義主動力的貢獻為

作用在模塊sk上的附加質(zhì)量力和附加質(zhì)量矩對廣義主動力的貢獻為

作用在模塊sk上的控制力矩對廣義主動力的貢獻為

水下自重構(gòu)機器人的廣義主動力和廣義慣性力可以表示為

水下自重構(gòu)機器人的Kane動力學(xué)方程為

4 建模實例

以水下自重構(gòu)機器人的典型構(gòu)形（水下四足行走構(gòu)形，如圖4所示）為例，采用本文提出的方法建立該構(gòu)形的動力學(xué)模型。

水下四足行走構(gòu)形的廣義坐標定義為

式中：x、y和z為身體質(zhì)心在慣性坐標系下的坐標；φ、θ和ψ表示身體相對于慣性坐標系的歐拉角；θj（j=1，2，…，8 ）表示關(guān)節(jié)角。

水下四足行走構(gòu)形的廣義速度為

圖4 水下四足行走構(gòu)形及其運動關(guān)系圖Fig.4 Underwater quadruped walking configuration and its kinematic relation graph

由圖 4 可知，水下四足行走構(gòu)形共有四條通路，分別為：s0、s1、s2和 s3；s0、s1、s4和 s5；s0、s6和 s7；s0、s8和s9。因此，水下四足行走構(gòu)形的通路矩陣為

考慮到s2、s4、s6和s8具有相似的運動學(xué)和動力學(xué)特征，我們將其歸為一類，統(tǒng)稱為大腿。同理，將s3、s5、s7和 s9歸為一類，統(tǒng)稱為小腿。 按照（16）～（24）式計算偏速度、偏角速度、廣義慣性力和廣義主動力，并按（25）式和（26）式寫出最終的Kane動力學(xué)方程。

采用CPG設(shè)計水下四足行走構(gòu)形的控制算法[15]，將足與地面的接觸問題等效為彈簧—阻尼模型，實現(xiàn)水下行走。圖5給出了水下四足行走構(gòu)形行走過程中，身體質(zhì)心的運動軌跡。地面坐標z=0，豎直向下為正。x0表示前進方向的位移，z0表示豎直方向的位移，初始階段（t=0）x0=0，z0=-1.2。由于行走過程中四肢交替運動，身體質(zhì)心在豎直方向會產(chǎn)生震蕩。

圖5 身體質(zhì)心的運動軌跡Fig.5 The trajectory of CM of body

5 結(jié) 論

本文系統(tǒng)地介紹了水下自重構(gòu)機器人的統(tǒng)一構(gòu)形描述方法；考慮到水下自重構(gòu)機器人構(gòu)形的復(fù)雜多變，提出了先對單個模塊進行水動力計算，然后將所有模塊的水動力相加來近似計算水下自重構(gòu)機器人的水動力；給出了基于統(tǒng)一構(gòu)形描述和Kane動力學(xué)的水下自重構(gòu)機器人的通用動力學(xué)建模方法。以水下四足行走構(gòu)形為例，驗證了水下自重構(gòu)機器人通用動力學(xué)建模方法的有效性。

[1]Xu Xusong,Ge Tong,Zhu Jimao.Optimal topological transformation of underwater modular self-reconfigurable robots[J].Robotica,2005,23:581-594.

[2]李家旺,葛 彤,吳 超,王旭陽.水下自重構(gòu)機器人的自動對接控制[J].上海交通大學(xué)學(xué)報,2012,46(2):190-194.Li J W,Ge T,Wu C,W X Y.Docking control of underwater self-reconfigurable robot[J].Journal of Shanghai Jiaotong U-niversity,2012,46(2):190-194.

[3]Yang K,Wang X Y,Ge T,Wu C.Dynamic model of underwater snake-like robot using Kane’s method[J].Journal of Shanghai Jiaotong University,2014,19(2):146-154.

[4]Yang K,Wang X Y,Ge T,Wu C.A dynamic model of an underwater quadruped walking robot using Kane’s method[J].Journal of Shanghai Jiaotong University,2014,19(2):160-168.

[5]Maladen R D,Ding Y,Umbanhowar P B,et al.Undulatory swimming in sand:Experimental and simulation studies of a robotic sandfish[J].The International Journal of Robotics Research,2011,30(7):793-794.

[6]MA S G,Tadokoron.Analysis of creeping locomotion of a snake-like robot on a slope[J].Autonomous Robots,2006,20(1):15-23.

[7]Guo L,Liao Q Z,Wei S M.Dynamic modeling of bicycle robot and nonlinear control based on feedback linearization of MIMO systems[J].Journal of Beijing University of Post and Telecommunications,2007,30(1):80-84.

[8]Baruh H.Another look at the describing equations of dynamics[J].Journal of the Chinese Society of Mechanical Engineering,2000,21(1):15-23.

[9]夏 丹,陳維山,劉軍考,等.基于Kane方法的仿魚機器人波狀游動的動力學(xué)建模[J].機械工程學(xué)報,2009,45(6):41-49.Xia D,Chen W S,Liu J K,et al.Dynamic modeling of a fishlike robot with undulatory motion based on Kane’s method[J].Journal of Mechanical Engineering,2009,45(6):41-49.

[10]李新友,陳五一,韓先國.基于Kane方程的SUPS/S并聯(lián)機構(gòu)動力學(xué)研究[J].機床與液壓,2011,39(13):1-5.Li X Y,Chen W Y,Han X G.Dynamic analysis of a 3UPS/S parallel mechanism based on Kane equations[J].Machine Tool and Hydraulics,2011,39(13):1-5.

[11]沈 飛,曹志強,徐 德,等.基于Kane方法的機器海豚動力學(xué)建模及速度優(yōu)化方法[J].自動化學(xué)報,2012,38(8):1247-1256.Shen F,Chao Z Q,Xu D,et al.A dynamic model of robotic dolphin based on Kane method and its speed optimization[J].Acta Automatica Sinica,2012,38(8):1247-1256.

[12]Cheng G,Shan X L.Dynamics analysis of a parallel hip joint simulator with four degree of freedoms(3R1T)[J].Nonlinear Dynamics,2012,70(4):2475-2486.

[13]Lin C C,Chen R C,Li T L.Experimental determination of the hydrodynamic coefficients of an underwater manipulator[J].Journal of Robotic System,1999,16(6):329-338.

[14]Muthyunjaya T S.Kinematic structure of mechanisms revisited[J].Mechanism and Machine Theory,2003,38(4):279-320.

[15]Yang K,Wang X Y*,Ge T,Wu C.Simulation platform of underwater quadruped walking robot based on motion genesis Kane 5.3 and central pattern generator[J].Journal of Harbin Institute of Technology,2014,21(3):24-29.