基于Kane動力學和通路矩陣的水下自重構機器人建模

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

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

0 引 言

水下自重構機器人由相同模塊組成，可以根據環境和任務需要通過自重構策略動態重構出所需的構形。水下自重構機器人構形的變化增強了其在復雜多變的水下環境中的適應能力[1-2]，但同時增加了建模的難度。與常規水下機器人AUV和ROV相比，水下自重構機器人的運動學和動力學顯然要復雜很多[3-4]。

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

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

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

1 水動力計算

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

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

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

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

k

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

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

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

k

2 統一構形描述

水下自重構機器人可以根據任務需求和海洋環境信息動態重構出不同構形，因此，很難對水下自重構機器人的所有構形進行統一的描述。為了描述水下自重構機器人的所有構形，我們需要解決以下問題：對模塊的設計要求、運動關系描述、可達矩陣和通路矩陣。

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

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

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

圖2 水下自重構機器人構形和運動關系圖Fig.2 Configuration and kinematic relation graph of the underwater self-reconfigurable robot

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

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

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

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

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

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

利用圖論中的圖形搜索技術和機構的運動關系描述，我們可以用統一的數學公式來描述水下自重構機器人的所有構形。

3 水下自重構機器人Kane動力學建模

3.1 運動學分析

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

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

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

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

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

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

公式（9）可重寫為

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

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

偏速度和偏角速度為

3.2 動力學分析

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

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

模塊sk的廣義慣性力為

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

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

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

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

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

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

水下自重構機器人的Kane動力學方程為

4 建模實例

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

水下四足行走構形的廣義坐標定義為

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

水下四足行走構形的廣義速度為

圖4 水下四足行走構形及其運動關系圖Fig.4 Underwater quadruped walking configuration and its kinematic relation graph

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

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

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

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

5 結 論

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

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

[2]李家旺,葛 彤,吳 超,王旭陽.水下自重構機器人的自動對接控制[J].上海交通大學學報,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方法的仿魚機器人波狀游動的動力學建模[J].機械工程學報,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并聯機構動力學研究[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方法的機器海豚動力學建模及速度優化方法[J].自動化學報,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.