李炳志? 王爽

摘? 要：偽速度的引入導(dǎo)致了拉氏方程的廣義化，使得通過歐拉-龐卡萊方程建立多體系統(tǒng)的動力學(xué)模型，實現(xiàn)了動力學(xué)的全局描述，并保持了潛在理論結(jié)構(gòu)和拉氏方程的優(yōu)點。并且其慣量矩陣相對簡單，對于完整或非完整約束系統(tǒng)都適用?？臻g算子代數(shù)中的移位算子結(jié)合了關(guān)聯(lián)矩陣、通路矩陣和低序體陣列的特點，而且形式更簡單，更容易在計算機上實現(xiàn)，最重要的是它包含了運動學(xué)和動力學(xué)遞推關(guān)系。將其引入動能函數(shù)的計算，提高了效率，避免了重復(fù)計算、交叉運算。最后給出了算例及仿真。

關(guān)鍵詞：偽速度；歐拉-龐卡萊方程；移位算子；高效率；動力學(xué)模型

中圖分類號：TP241? 文獻(xiàn)標(biāo)識碼：A? 文章編號：2096-4706（2023）24-0147-06

High Efficient Dynamics Modeling of Mechanical Multibody System Based On

Euler-Poincare Equation

HAO Yongjiang1， LI Bingzhi2， WANG Shuang3

（1.Kunshan Key Industry Development and Promotion Center， Suzhou? 215300， China; 2.China National Heavy Duty Truck Group Co.， Ltd.， Ji'nan? 250000， China; 3.Nanjing University of Information Science & Technology， Nanjing? 210044， China）

Abstract： The introduction of quasi-velocities leads to a generalization of Lagrange's equation， and the dynamics model of multi-body system established by Euler-Poincare equation achieves global description of the dynamics， and keeps the advantages of underlying theoretical structure and the Lagrange's equation. Its inertia matrix is relatively simple， which can be application to holonomic or non-holonomic constrained systems. The shift operator of spatial operator algebra combines the features of correlation matrix， access matrix and lower body arrays， and its simple structure is easy to be achieved on computer. More importantly， it contains recurrence relation of kinematics and dynamics. It is introduced into the calculation of the kinetic energy function， which improves the efficiency， avoids double counting and cross operation. Finally， the calculation examples and the simulation are given.

Keywords： quasi-velocities; Euler-Poincare equation; shift operator; high efficiency; dynamics model

0? 引? 言

過去十多年來，不得不面對一系列控制設(shè)計問題—多體復(fù)雜機電系統(tǒng)，其中非線性起著重要的作用，幸運的是非線性控制系統(tǒng)分析的幾何理論在20世紀(jì)八九十年代取得了實質(zhì)性的進(jìn)展，為解決許多需要，提供了關(guān)鍵性的概念化的工具，但是誠于許多控制工程師所承認(rèn)的，在工程實踐上，快速建模，計算和實現(xiàn)是主要關(guān)心的事情，這一問題又提出了如何快速地對復(fù)雜多體系統(tǒng)進(jìn)行數(shù)學(xué)建模，因此建立和處理復(fù)雜的模型對于設(shè)備和控制器都是首先需要的。

然而，面對中-高等多體鏈?zhǔn)较到y(tǒng)，建立準(zhǔn)確的數(shù)學(xué)模型也相當(dāng)復(fù)雜和困難。自然，利用計算機進(jìn)行自動化推導(dǎo)的興趣一直在增加，大多數(shù)實際工程中主要集中于鏈?zhǔn)交驑涫浇Y(jié)構(gòu)，這是一類適用于重要機器人和車輛的系統(tǒng)。但是，許多系統(tǒng)并非樹形系統(tǒng)，它們會帶有閉環(huán)以及其形式的代數(shù)和（或）微分約束，需要加到打開的樹形系統(tǒng)上，典型的例子是機器人手爪的抓取，或者車輛帶有滾輪，這種系統(tǒng)附加的復(fù)雜化增大了計算機組裝方程的規(guī)模。

Poincare方程[1]也叫作偽坐標(biāo)下的拉格朗日方程或者歐拉—龐卡來方程[2]，龐卡來方程包含了潛在的鏈?zhǔn)浇Y(jié)構(gòu)特點和模塊化的優(yōu)點，但它們常常保持自然本質(zhì)屬性，鏈?zhǔn)椒绞浇恿W(xué)方程相比拉氏方程更簡單，此外它們的組裝更容易，適于自動使他們成為“工業(yè)強度”系統(tǒng)的實際選擇[3-6]。

然而，符號計算，因為它對于非線性，參數(shù)依賴的系統(tǒng)是基本的，而它對于控制工程師是相當(dāng)新的工具，符號計算不取代數(shù)值計算，它而是補充和加強數(shù)值計算，認(rèn)識符號和數(shù)值計算的區(qū)別以及如何更好地將它們集成起來是一項有意義的挑戰(zhàn)，采用符號計算于以下幾個目的：基本的數(shù)學(xué)計算（實現(xiàn)坐標(biāo)變換、計算李括號）；建立顯示的數(shù)學(xué)模型；簡化模型（通過Taylor線性化，對稱化簡化）；產(chǎn)生數(shù)字仿真指令；完成非線性控制的構(gòu)造（計算逆系統(tǒng)或完成反饋線性化）；實現(xiàn)控制器的數(shù)值指令生成。

綜上所述，本文介紹樹結(jié)構(gòu)的運動學(xué)和歐拉—龐卡萊符號建模以及模塊化的建模并以一個實例為對象和仿真。

1? 偽速度

機械多體系統(tǒng)常假設(shè)為鏈?zhǔn)綐浣Y(jié)構(gòu)，即使事實上帶有閉環(huán)，也可以方便的建造相應(yīng)的潛在樹模型（打開閉環(huán)），再加上附加的約束條件。所考慮的系統(tǒng)是剛體組成的，通過關(guān)節(jié)將剛體聯(lián)系起來，多體系統(tǒng)是由剛體通過關(guān)節(jié)相連而組成。每個關(guān)節(jié)有一組速度變量及其形位（configuration）參數(shù)，這些參數(shù)的個數(shù)等于關(guān)節(jié)的自由度數(shù)，相應(yīng)于關(guān)節(jié)參數(shù)組成的廣義坐標(biāo)定義為q。

考慮完整約束的系統(tǒng)，其可能形位對應(yīng)于光滑流形M，（維數(shù)為m）稱之為形位流形。M上的局部坐標(biāo)定義了系統(tǒng)的形位，稱之為廣義坐標(biāo)[7]。任何在時間間隔（t1，t2）內(nèi)系統(tǒng)的運動，跟蹤了M上的一個軌跡，該軌跡通過局部q（t）：[t1，t2]→M，任何q ∈ M上的一點有廣義速度（dq/dt）屬于M的切空間，在q這點，用TqM來描述。動力系統(tǒng)的狀態(tài)空間是2m維流形，，稱之為余切從。

在q ∈ M形位處系統(tǒng)的虛位移是一個無限小位移δq，它使系統(tǒng)轉(zhuǎn)移到一個許可形位q′ ∈ M，q′ = q + δq，很顯然δq是虛位移的條件為：當(dāng)且僅當(dāng)它是無限小的，而且δq ∈ TqM，若系統(tǒng)在形位q處有一個廣義力Q的作用，則在虛位移δq作用下的虛功為δw = QTδq。

設(shè)M是m維形位流形，對于拉氏系統(tǒng)，又設(shè)V1，…，Vm構(gòu)成系統(tǒng)的m個線性獨立的矢量場，作用在M上，于是每個換拉子或李括號可表達(dá)為：

的確，這些系數(shù)容易在局部坐標(biāo)系中計算，定義：

設(shè)q（t）：[t1，t2]→M是一個光滑軌跡，則q（t）定義了在點q（t） ∈ M處軌跡的切向量，所以我們總是能夠?qū)（t）表達(dá)為切向量vi的線性組合（i=1，2，…，m）

在局部坐標(biāo)中，m維流形上的矢量場可以想象為m維列向量，余矢量場為m維列向量，用這種辦法來計算，其中：

變量p稱為偽速度。由于這些量是“速度”，我們或許試圖將它們與一組坐標(biāo)π聯(lián)系起來，使? 成立，但這并不能總是得到保證，考察（4），有：

δπ = U（q）δp

但是實際上，右手邊（每個δπi）不總是可微的（應(yīng)為積分）。

2? 樹形拓?fù)浣Y(jié)構(gòu)描述

一個多體系統(tǒng)可以視為潛在的樹結(jié)構(gòu)，附加的代數(shù)或微分約束方程。描述MBS樹和數(shù)據(jù)結(jié)構(gòu)，則展示如何計算樹中任意位置在參考系中的形位坐標(biāo)和速度，一個樹定義為一組鏈，每個鏈?zhǔn)加诟鶆傮w。帶有閉環(huán)的系統(tǒng)可由樹系統(tǒng)加上約束實現(xiàn)，一個樹有n的剛體和n個關(guān)節(jié)組成，每個系統(tǒng)包括基架參考坐標(biāo)系，它由體“O”來定義，否則體和鉸可以任意編號。

獨立數(shù)據(jù)體的結(jié)構(gòu)描述如下，關(guān)節(jié)和體由他們在數(shù)據(jù)列表中的位置隱含地編號，每個物體包含一個唯一的“向內(nèi)的”接點（in board node），對應(yīng)于（外邊的）關(guān)節(jié)，經(jīng)過此關(guān)節(jié)，該物體與樹的內(nèi)部分支聯(lián)系起來，或聯(lián)到根上去（體O），每個物體也可能包含“向外的”接點（outboard nodes），這些向外的接點區(qū)分了體的地址（locations），這些地址可能與關(guān)節(jié)地址（關(guān)節(jié)的向內(nèi)邊）有關(guān)，或與傳感器地址有關(guān)，或與外部作用點有關(guān)，或任意其他作用點有關(guān)。既然一個關(guān)節(jié)把樹連接到根（根的接點視為體O的外向接點），則必然有n-1個外向接點，存在于n個物體上，對應(yīng)于余下n-1的個關(guān)節(jié)，這些就是n個向外的接點。向外關(guān)節(jié)接點必然依1到n的次序進(jìn)行編號，且對應(yīng)于相應(yīng)的關(guān)節(jié)序號，特定與關(guān)節(jié)有關(guān)的序號不是基本的，但是通常根接點的序號被賦予序號1，而余下的外向接點可以任意編號，向內(nèi)接點不必分配序號[8，9]。

一個樹由一組鏈組成，如一個樹：0，1，2，4；0，1，2，3，5；0，1，2，3，6。所有的鏈皆從0號剛體開始，因此不必將它列入。不過體序列本身不足以定義一個樹，例如體5和6皆連到了體3上，但是她們通過不同的鉸連上去。此中信息由定義每個鏈為一組有序列表來提供；每組包含一個體和它的向內(nèi)鉸接點，即{向內(nèi)鉸好，體號}。例如下述三鏈：{{1，1}，{2，2}，{5，4}}；{{1，1}，{2，2}，{3，3}，{4，5}}；{{1，1}，{2，2}，{3，3}，{6，6}}；鏈1包含了體1，2，4；體1通過鉸1連到參考系，體2通過鉸2連到體1，體4通過鉸5連到體4，數(shù)據(jù)表明體5在鉸4處連到了體3（鉸4在第二條鏈上），體6通過鉸6（在第三條鏈上）連到體3上。

以圖1所示的樹形多體系統(tǒng)為例，對多體系統(tǒng)的各種拓?fù)浣Y(jié)構(gòu)描述方法進(jìn)行比較。首先，利用羅伯森和維騰堡首先提出利用圖論的方法，建立其關(guān)聯(lián)矩陣S和通路矩陣T分別為：

根據(jù)文獻(xiàn)[10]方法，建立圖1系統(tǒng)的低序體陣列見表1。

根據(jù)空間算子代數(shù)標(biāo)號規(guī)則[11]，將圖1系統(tǒng)進(jìn)行重行標(biāo)號（如圖2所示），對于分枝的編號L1，…，Ll各分枝中體的編號遵照規(guī)則標(biāo)號與關(guān)聯(lián)矩陣方法恰好相反，即內(nèi)側(cè)分枝號及體編號大于外側(cè)分枝及體編號。建立其移位算子為：

其中，，。

考察由k+1各剛體組成的單鏈，通過關(guān)節(jié)連接起來，如圖3和4所示，體由0到k的序號編號，0表示參考剛體，同時表示任意常規(guī)的慣性參考系，第k號關(guān)節(jié)將k-1剛體在點Ok-1與k號剛體在點Ok點聯(lián)系起來。

圖3? 由k+1個剛體組成序號從0到k的k關(guān)節(jié)，序號從1到k的單鏈MBS

圖4? 在任意k號連桿上，向內(nèi)向外關(guān)節(jié)鉸接點定義為Ok和Ck，體固聯(lián)坐標(biāo)系以O(shè)k為其原點

設(shè)F k表示固定在體上的參考系，以O(shè)k為其原點，rCOk表示從Ok到Ck的矢量，r k表示Ok到Ok+1的矢量，皆定義在F k中，我們將采用坐標(biāo)的特別說明，在其中表示矢量，或者在切空間中描述，以上標(biāo)加以“k”標(biāo)識，而與坐標(biāo)無關(guān)就不加任何上標(biāo)。有時需要應(yīng)用一個固定在體k的坐標(biāo)系，系由F k平移，但是以Pk點而不是Ok點為其原點，此時我們采用Fpkk的記號，設(shè)rpok定義了從O到P的矢量，它也是F k中的矢量，則從F k到FPKk的平移變換產(chǎn)生形位矩陣：

第k個中關(guān)節(jié)有nk，1≤nk≤6個自由度，它們通過nk個坐標(biāo)q（k）以及相應(yīng)的nk個偽速度β（k）以及一個形位矩陣Xk[q（k）]，希望計算一個參考系的歐幾里得形位矩陣，該系固定在最后一個剛體上，以末端接點為其原點，例如：圖4的系統(tǒng)可以有5個接點，我們可以得到相對于定向空間坐標(biāo)系的形位：

關(guān)節(jié)1的作用→移動到C1→…→關(guān)節(jié)k的作用→移動到Pk：

式（8）可被修正，用來計算，鏈或樹中不同體坐標(biāo)系下任意兩個接點的形位，為了使之對樹適用這個計算需要一個簡單程序來找出聯(lián)系兩個接點的鏈。

3? 效率動力學(xué)建模

一般情況下，多體系統(tǒng)可視為潛在的樹結(jié)構(gòu)，其上可附加作用有代數(shù)的和（或）微分的約束。樹形系統(tǒng)都是由一組鏈組成，本部分通過鏈?zhǔn)较到y(tǒng)來說明通過龐卡萊方程建立高效率動力學(xué)方程的過程，可進(jìn)一步擴展到樹形系統(tǒng)。建立拉格朗日方程及龐卡來方程的關(guān)鍵是建立動能函數(shù)，首先根據(jù)空間算子代數(shù)理論[12]，以n個連桿的串聯(lián)機械臂（如圖5所示）為例，得到空間速度的遞推關(guān)系如下。

圖5中k+1個物體組成的鏈，Rordriguez定義了C點的空間速度，在以C為其原點的坐標(biāo)系下，Vc = [ω，vc]，vc表示C點的速度，ω表示連桿的角速度，設(shè)O為同一物體上的另一點，rCO表示C對于O在體坐標(biāo)系中的位置，則C點的空間速度對于O點的速度存在關(guān)系：

其中：。

其轉(zhuǎn)置：

關(guān)節(jié)k的關(guān)節(jié)映射矩陣H（k） ∈ R6×nk，故：

于是，對（10）到（12）的重復(fù)導(dǎo)致了如下速度遞推關(guān)系，將其在坐標(biāo)說明（coordinate specific notation）中表示：

式中，上標(biāo)i表示第i坐標(biāo)系，假定H（k），β（k）定義在F k坐標(biāo)系中，而V（k-1）在F k-1坐標(biāo)系中計算，于是容易在k坐標(biāo)系中計算出V（k）：

若V 0（0）給定，則（14）使我們能夠作遞推計算，對于k = 1…k，遞推計算出F k原點的速度，以及其角速度，它們二者在F k坐標(biāo)系中的表示，隨后我們設(shè)V 0（0） = 0。有點濫用說明，容易定義：

則式（15）改寫為式（16）：

考慮第k個剛體連桿，設(shè)Icm（k）表示其關(guān)于質(zhì)心在坐標(biāo)系F k下的慣量張量，m（k）表示質(zhì)量，a（k）表示從質(zhì)心到任意點O的位置矢量關(guān)于質(zhì)心的空間慣量，MCM，對于O，MO為：

式中：IO是對于O的慣性張量。

空間速度和空間慣性矩陣，由此對于整個鏈而言，就容易方便地構(gòu)造出來，定義空間速度和關(guān)節(jié)偽速度：

V = [V T（1），…，V T（n）]，β = [β T（1），…，β T（n）]（18）

因此得到：

其中：

由連桿1到n組成鏈的動能函數(shù)為：

其中，，。

為：

通過上述定義和構(gòu)造得出偽速度形式的動能函數(shù)：

因此，得到簡化形式的龐卡方程為：

其中：，，。

u（q）表示勢能函數(shù)，Qp表示偽速度坐標(biāo)系下的廣義力，與拉氏方程相比龐卡萊方程有重要的優(yōu)點，一個明顯又實際的是慣量矩陣相對簡單，并且拉氏方程本質(zhì)上構(gòu)造了一個局部表示，無論對于何種局部坐標(biāo)，而龐卡萊方程可以導(dǎo)致一個動力學(xué)的全局描述。

4? 算例：數(shù)值計算

各連桿坐標(biāo)系如圖6所示，系統(tǒng)的Denavit-Hartenberg參數(shù)列表如表2所示。

假設(shè)各桿件質(zhì)量分別m1 = 2.1 kg，m2 = 2.5 kg，m3 = 3.0 kg，L1 = 2.1 m，L2 = 2.5 m，L3 = 3.0 m，假設(shè)沒有外力矩情況下，仿真得到各關(guān)節(jié)角度及偽速度變化曲線如圖7。

5? 結(jié)? 論

偽速度的引入導(dǎo)致了拉氏方程的廣義化，使得通過歐拉-龐卡萊方程建立多體系統(tǒng)的動力學(xué)模型，實現(xiàn)了動力學(xué)的全局描述。并且其慣量矩陣相對簡單，對于完整或非完整約束系統(tǒng)都適用。將空間算子代數(shù)中的移位算子引入動能函數(shù)的計算，提高了效率，同時，避免了重復(fù)計算、交叉運算。最后給出了算例及仿真結(jié)果。

參考文獻(xiàn)：

[1] EMELYANOV S V，KOROVIN S K，LEVANTOVSKY L V. A Drift Algorithm in Control of Uncertain Process [J].Problems of Control and Information Theory，1986，15（6）：425-438.

[2] YONG K D，KWATNY H G. Variable Structure Servomehanism and its Applicationto Overspeed Protection Control [J].Automatica，1982，18（4）：385-400.

[3] MARINO R. High Gain Feedback NonLinear Control System [J].International Joural of Control，1985，42（6）：1369-1385.

[4] KWATNY H G. Variable Structure of AC Drives [M].Variable Structure Control for Robotics Aerospace Applicatons，Amsterdam：Elsevier，1993.

[5] YOUNG K K，KOKOTOVIC P，UTKIN V. A singular perturbation analysis of high-gain feedback systems [J].IEEE Transactions on Automatic Control，1977，22（6）：931-938.

[6] SLOTIONE J J，SASTRY S S. Tracking Control of Nonlinear Systems Using Sliding Surfaces，With Application to Robot Manipulators [J].Internatonal Journal of Control，1983.38（2）：465-492.

[7] JIAN A，RODRIGUEZ G. Computational robot dynamics using spatial operators [C]//IEEE International Conference on Robotics and Automation.San Francisco，IEEE，2000：843-849.

[8] FEATHERSTONE R. Efficient Factorization of the Joint-Space Inertia Matrix for Branched Kinematic Trees [J].The International Journal of Robotics Research，2005，24（6）：487-500.

[9] 劉延柱，洪嘉振，揚海興.多剛體動力學(xué) [M].北京：高等教育出版社，1989.

[10] 馬霞，張晉珠，李燦，等.應(yīng)用R軟件的ARIMA模型教學(xué)案例設(shè)計 [J].福建電腦，2022，38（11）：113-117.

[11] RODRIGUEZ G，JIAN A，KREUTZ-DELGADO K. Spatial Operator Algebra for multibody system dynamics [J].Journal of the Astronautical Sciences，1992，40：27-50.

[12] FEATHERSTONE R. Robot Dynamics Algorithms [M].New York：Springer，1987.

作者簡介：郝永江（1978—），男，漢族，山東牟平人，中級工程師，博士研究生，研究方向：機械控制；李炳志（1980—），男，漢族，山東德州人，工程師，碩士研究生，研究方向：汽車電子；王爽（1999—），男，漢族，江蘇宿遷人，碩士研究生在讀，研究方向：視覺SLAM。