基于傳遞矩陣法的足式機(jī)器人四連桿腿部機(jī)構(gòu)正向與逆向動(dòng)力學(xué)分析

趙鑫宇 宋延松 朱曉蕙 剛憲約 柴匯

文章編號(hào)：1671-3559（2024）01-0115-08DOI：10.13349/j.cnki.jdxbn.20231223.001

摘要：為了保證足式機(jī)器人腿部機(jī)構(gòu)的控制準(zhǔn)確性與實(shí)時(shí)性，利用向量代數(shù)方法，對(duì)足式機(jī)器人四連桿腿部機(jī)構(gòu)進(jìn)行幾何運(yùn)動(dòng)學(xué)分析；根據(jù)有限元形函數(shù)理論，建立足式機(jī)器人四連桿腿部機(jī)構(gòu)典型構(gòu)件的質(zhì)量離散方法；在幾何運(yùn)動(dòng)學(xué)分析的基礎(chǔ)上，基于力矩平衡原理，分別進(jìn)行正向與逆向動(dòng)力學(xué)分析；利用線性變換原理并結(jié)合傳遞矩陣法，建立足式機(jī)器人四連桿腿部機(jī)構(gòu)正-逆向動(dòng)力學(xué)統(tǒng)一模型，并利用Adams軟件建立足式機(jī)器人四連桿腿部機(jī)構(gòu)虛擬樣機(jī)模型，進(jìn)行正向與逆向動(dòng)力學(xué)仿真實(shí)例分析。結(jié)果表明，所建立的足式機(jī)器人四連桿腿部機(jī)構(gòu)正-逆向動(dòng)力學(xué)統(tǒng)一模型與虛擬樣機(jī)模型3個(gè)油缸力與3個(gè)方向足底力的誤差分別小于1%與3%，驗(yàn)證了所建立的足式機(jī)器人四連桿腿部機(jī)構(gòu)正-逆向動(dòng)力學(xué)統(tǒng)一模型能夠精確地求解油缸力與足底力。

關(guān)鍵詞：足式機(jī)器人；腿部機(jī)構(gòu)；動(dòng)力學(xué)分析；傳遞矩陣法

中圖分類號(hào)：TP242.3

文獻(xiàn)標(biāo)志碼：A

開放科學(xué)識(shí)別碼（OSID碼）：

Forward and Reverse Dynamics Analyses on Four-bar Linkage

Leg Mechanism of Legged Robots Based on Transfer Matrix Method

ZHAO Xinyu1， SONG Yansong1， ZHU Xiaohui1， GANG Xianyue1， CHAI Hui2

（1. School of Transportation and Vehicle Engineering， Shandong University of Technology， Zibo 255000， Shandong， China;

2. Center for Robotics， Shandong University， Jinan 250061， Shandong， China）

Abstract： To ensure control accuracy and instantaneity of leg mechanism for legged robots， geometric kinematic analysis on four-bar linkage leg mechanism of legged robots was carried out by using vector algebra method. A mass dispersion method for typical bars of four-bar linkage leg mechanism of legged robots was established according to finite element form function theory. On the basis of geometric kinematic analysis and principle of moment balance， forward and reverse dynamic analyses were carried out respectively. A unified forward-reverse dynamics model for four-bar linkage leg mechanism of legged robots was established by using principle of linear transformation and combining transfer matrix method. A virtual prototype model for four-bar linkage leg mechanism of legged robots was established by using Adams software for example analysis of forward and reverse dynamics simulation. The results show that errors of three cylinder forces and plantar forces in three directional between the established unified forward-reverse dynamics model and the virtual prototype model for four-bar linkage leg mechanism of legged robots are less than 1% and 3% ， which verifies that the established unified forward-reverse dynamics model for four-bar linkage leg mechanism of legged robots can accurately solve cylinder force and plantar force.

Keywords： legged robot; leg mechanism; dynamics analysis; transfer matrix method

收稿日期：2022-09-25????????? 網(wǎng)絡(luò)首發(fā)時(shí)間：2023-12-25T10：35：24

基金項(xiàng)目：國家自然科學(xué)基金項(xiàng)目（62073191）；山東省重大科技創(chuàng)新工程項(xiàng)目（2019JZZY020317）；山東省自然科學(xué)基金項(xiàng)目（ZR2020ME140）

第一作者簡介：趙鑫宇（1998—）， 男， 山東淄博人。 碩士研究生， 研究方向?yàn)閯?dòng)力學(xué)分析仿真、結(jié)構(gòu)優(yōu)化。E-mail： zhaoxinyu7419@163.com。

通信作者簡介：剛憲約（1977—），男，山東濟(jì)南人。教授，博士，碩士生導(dǎo)師，研究方向?yàn)榻Y(jié)構(gòu)優(yōu)化、車輛系統(tǒng)動(dòng)力學(xué)。E-mail： gangxianyue@

sdut.edu.cn。

網(wǎng)絡(luò)首發(fā)地址：https：//link.cnki.net/urlid/37.1378.N.20231223.1236.002

足式機(jī)器人著地點(diǎn)是離散的，可以在地面上選擇最優(yōu)位置，并且能夠跨越障礙，幾乎可以適應(yīng)各種復(fù)雜地形，因此在制造業(yè)、服務(wù)業(yè)、軍事、航天、核工業(yè)等領(lǐng)域具有非常廣闊的應(yīng)用前景［1］。機(jī)器人動(dòng)力學(xué)主要研究機(jī)器人末端位置所受載荷與各關(guān)節(jié)所需驅(qū)動(dòng)力之間的關(guān)系，動(dòng)力學(xué)分析可以準(zhǔn)確地判斷機(jī)器人在工作時(shí)腿部機(jī)構(gòu)的受力，為后期優(yōu)化和高效穩(wěn)定控制奠定基礎(chǔ)。

為了對(duì)機(jī)器人進(jìn)行實(shí)時(shí)控制與結(jié)構(gòu)設(shè)計(jì)， 改善運(yùn)動(dòng)性能， 許多學(xué)者利用不同方法對(duì)機(jī)器人進(jìn)行動(dòng)力學(xué)建模。 崔敏其［2］利用拉格朗日公式對(duì)SCARA型機(jī)器人的動(dòng)力學(xué)模型進(jìn)行了詳細(xì)推導(dǎo)；劉芳華等［3］利用虛功原理對(duì)機(jī)器人進(jìn)行動(dòng)力學(xué)建模， 研究關(guān)節(jié)驅(qū)動(dòng)力矩與結(jié)構(gòu)尺寸、位置等之間的關(guān)系；張鐵等［4］基于牛頓-歐拉方法對(duì)機(jī)器人建立動(dòng)力學(xué)方程， 得出機(jī)器人運(yùn)動(dòng)時(shí)的關(guān)節(jié)力矩， 為動(dòng)力學(xué)控制提供了基礎(chǔ)計(jì)算公式；Wu等［5］利用拉格朗日公式和能量法建立動(dòng)力學(xué)模型， 并利用Adams軟件仿真驗(yàn)證動(dòng)力學(xué)模型的準(zhǔn)確性。 正向動(dòng)力學(xué)分析是指以腿部機(jī)構(gòu)各油缸的力和位移為輸入， 預(yù)測足端輸出的力和位移；逆向動(dòng)力學(xué)分析是指依據(jù)期望得到的足端輸出力和位移， 反求各油缸應(yīng)施加的輸入力和位移。

已有研究中正向與逆向動(dòng)力學(xué)分析需要2種完全不同的數(shù)學(xué)方程，計(jì)算過程復(fù)雜，影響控制的準(zhǔn)確性和實(shí)時(shí)性。本文中基于力矩平衡原理，對(duì)四連桿腿部機(jī)構(gòu)進(jìn)行正向與逆向動(dòng)力學(xué)分析，利用線性變換原理并結(jié)合傳遞矩陣法，將正向與逆向動(dòng)力學(xué)分析合二為一，建立足式機(jī)器人四連桿腿部機(jī)構(gòu)（簡稱腿部機(jī)構(gòu)）正-逆向動(dòng)力學(xué)統(tǒng)一模型。

1? 腿部機(jī)構(gòu)幾何運(yùn)動(dòng)學(xué)分析

對(duì)足式機(jī)器人的腿部機(jī)構(gòu)進(jìn)行幾何運(yùn)動(dòng)學(xué)分析，研究腿部機(jī)構(gòu)在運(yùn)動(dòng)過程中幾何參數(shù)與運(yùn)動(dòng)參數(shù)之間的關(guān)系，為動(dòng)力學(xué)分析提供理論支持［6］。

足式機(jī)器人的腿部機(jī)構(gòu)包括側(cè)擺油缸、髖關(guān)節(jié)、大腿與小腿， 如圖1所示。 側(cè)擺油缸控制腿部機(jī)構(gòu)繞髖關(guān)節(jié)側(cè)向擺動(dòng)， 左端與機(jī)架鉸接， 右端與髖關(guān)節(jié)鉸接， 髖關(guān)節(jié)鉸接在機(jī)架上。 大腿和小腿也分別由2個(gè)油缸通過連桿機(jī)構(gòu)實(shí)現(xiàn)在腿部機(jī)構(gòu)平面內(nèi)的縱向擺動(dòng)。 由于腿部髖關(guān)節(jié)的側(cè)擺和腿部機(jī)構(gòu)的縱擺的控制相互獨(dú)立， 因此可將幾何運(yùn)動(dòng)學(xué)分析分2步進(jìn)行：首先在腿部縱擺平面內(nèi)進(jìn)行幾何運(yùn)動(dòng)學(xué)分析，只考慮在腿部機(jī)構(gòu)的縱向擺動(dòng)自由度， 根據(jù)機(jī)構(gòu)桿件長度、油缸工作行程和關(guān)節(jié)轉(zhuǎn)角計(jì)算腿部機(jī)構(gòu)各關(guān)節(jié)點(diǎn)在腿部隨體坐標(biāo)系的位置姿態(tài)［7］；然后在腿部側(cè)擺平面內(nèi)根據(jù)髖關(guān)節(jié)側(cè)擺角度利用坐標(biāo)變換， 計(jì)算整個(gè)腿部機(jī)構(gòu)鉸接點(diǎn)在全局坐標(biāo)系中的位置。

1.1? 腿部機(jī)構(gòu)坐標(biāo)系建立

對(duì)足式機(jī)器人整機(jī)和腿部機(jī)構(gòu)分別建立坐標(biāo)系，如圖2所示。

整機(jī)全局坐標(biāo)系XgloYgloZglo以髖關(guān)節(jié)側(cè)向擺動(dòng)支點(diǎn)O為坐標(biāo)原點(diǎn)，豎直向上為Zglo軸正方向，水平向右為Yglo軸正方向， Xglo軸滿足右手螺旋定則。其中，大腿與小腿所在的平面為腿部機(jī)構(gòu)縱向擺動(dòng)平面，垂直于紙面的投影為BK，O1N為側(cè)擺油缸，控制整個(gè)腿部機(jī)構(gòu)繞Xglo軸進(jìn)行側(cè)擺運(yùn)動(dòng)。由于腿部機(jī)構(gòu)平面存在繞髖關(guān)節(jié)側(cè)向擺動(dòng)支點(diǎn)O的側(cè)向擺動(dòng)，因此腿部隨體坐標(biāo)系會(huì)繞Xglo軸旋轉(zhuǎn)。

腿部隨體坐標(biāo)系XlegYlegZleg以腿部縱向擺動(dòng)支點(diǎn)B為坐標(biāo)原點(diǎn)，Xleg軸水平向右為正方向，Yleg軸垂直于腿部平面向外為正方向，Zleg軸豎直向上為正方向。其中桿件BI為大腿，桿件IK為小腿，AC為大腿油缸，推動(dòng)大腿實(shí)現(xiàn)繞Yleg軸的縱向擺動(dòng)；QH為小腿油缸，控制小腿進(jìn)行縱向擺動(dòng)。

1.2 ?縱向擺動(dòng)平面幾何運(yùn)動(dòng)學(xué)分析

首先對(duì)腿部縱向擺動(dòng)平面進(jìn)行幾何運(yùn)動(dòng)學(xué)分析，計(jì)算腿部機(jī)構(gòu)各關(guān)節(jié)鉸接點(diǎn)及連桿質(zhì)心在腿部隨體坐標(biāo)系中的位置。在圖2（b）中，A、B點(diǎn)分別固定在機(jī)架上，已知A、B點(diǎn)坐標(biāo)，機(jī)構(gòu)桿件長度、油缸長度及關(guān)節(jié)角計(jì)算其他鉸接點(diǎn)位置以及足端K的坐標(biāo)。

R點(diǎn)坐標(biāo)可由腿部隨體坐標(biāo)系中B點(diǎn)位置、桿件BR長度lBR和關(guān)節(jié)轉(zhuǎn)角θ1求得，即

RXleg

RYleg

RZleg=BXleg

BYleg

BZleg+lBRcos θ1

0

sin θ1 ，（1）

式中（RXleg， RYleg， RZleg

）、（BXleg， BYleg， BZleg）分別為腿部隨體坐標(biāo)系XlegYlegZleg中R、B點(diǎn)的坐標(biāo)。

同理，可分別求得H、I點(diǎn)在腿部隨體坐標(biāo)系中的坐標(biāo)。

根據(jù)I點(diǎn)坐標(biāo)、桿件IJK各段長度以及轉(zhuǎn)角θ2可求得J點(diǎn)坐標(biāo)，即

JXleg

JYleg

JZleg=IXleg

IYleg

IZleg+lIJcos θ2

0

sin θ2 ，（2）

式中：（JXleg， JYleg， JZleg）、（

IXleg， IYleg， IZleg）為腿部隨體坐標(biāo)系中J、I點(diǎn)坐標(biāo)；lIJ為桿件IJ長度。

K點(diǎn)求解方法同理。利用余弦定理，根據(jù)點(diǎn)A、B坐標(biāo)與桿件AC、BC的長度lAC、lBC，計(jì)算C點(diǎn)腿部隨體坐標(biāo)系中的坐標(biāo)（CXleg， CYleg， CZleg）。

同理，根據(jù)R、H點(diǎn)坐標(biāo)，以及油缸QH、桿件RQ的長度，計(jì)算Q點(diǎn)腿部隨體坐標(biāo)系中的坐標(biāo)（QXleg， QYleg， QZleg）。

由DB逆時(shí)針轉(zhuǎn)動(dòng)到CB的轉(zhuǎn)角φ2， lBC， 桿件BD長度lBD，以及B、C點(diǎn)坐標(biāo)，可求得由B點(diǎn)指向C點(diǎn)的單位向量

（nBC，Xleg， nBC，Yleg， nBC，Zleg），最終可得D點(diǎn)坐標(biāo)為

DXlegDYlegDZleg=

BXlegBYlegBZleg

+lBD

cosφ2sinφ200-sinφ2cosφ2

nBC，Xleg0nBC，Zleg。（3）

由R、Q點(diǎn)坐標(biāo)以及桿件ER長度lER、向量RQ的單位向量（nRQ，Xleg， nRQ，Yleg， nRQ，Zleg），可得E點(diǎn)坐標(biāo)為

EXlegEYlegEZleg=

RXlegRYlegRZleg

-lER

nRQ，XlegnRQ，YlegnRQ，Zleg 。（4）

桿件質(zhì)心位置需要利用桿件局部坐標(biāo)系計(jì)算。以桿件AB為例，建立桿件局部坐標(biāo)系XlocYlocZloc，選擇鉸接點(diǎn)A為桿件局部坐標(biāo)系的原點(diǎn)，Xloc軸正方向沿著桿件指向另一個(gè)鉸接點(diǎn)B，逆時(shí)針旋轉(zhuǎn)90°為Yloc軸正方向，根據(jù)右手螺旋定則確定Zloc軸正方向，如圖3所示。

兩節(jié)點(diǎn)線性桿件AB的局部坐標(biāo)系

根據(jù)各桿件質(zhì)心在桿件局部坐標(biāo)系中的位置，通過坐標(biāo)變換求出桿件質(zhì)心Cm在腿部隨體坐標(biāo)系中的坐標(biāo)為

Cm，Xleg

Cm，Yleg

Cm，Zleg=

AXleg

AYleg

AZleg+

nAB，Xleg-nAB，Zleg

00

nAB，ZlegnAB，Xleg

Cm，Xloc

Cm，Yloc

Cm，Zloc，（5）

其中nAB，Xleg

nAB，Yleg

nAB，Zleg=1lAB

BXleg-AXleg

BYleg-AYleg

BZleg-AZleg ，（6）

式中：（Cm，Xloc， Cm，Yloc， Cm，Zloc）

為桿件質(zhì)心Cm在桿件局部坐標(biāo)系中的坐標(biāo)；（nAB，Xleg， nAB，Yleg， nAB，Zleg）為桿件AB在腿部隨體坐標(biāo)系中的單位矢量；lAB為桿件AB長度；（AXleg， AYleg， AZleg）為鉸接點(diǎn)A在腿部隨體坐標(biāo)系中的坐標(biāo)。

1.3? 側(cè)向擺動(dòng)坐標(biāo)變換

由于腿部機(jī)構(gòu)縱擺平面在側(cè)擺油缸的作用下繞髖關(guān)節(jié)進(jìn)行側(cè)向擺動(dòng)，則各點(diǎn)的全局坐標(biāo)可利用坐標(biāo)轉(zhuǎn)換公式和側(cè)擺角θ3計(jì)算。

以K點(diǎn)在全局坐標(biāo)系中的位置為例，即

KXglo

KYgloKZglo=

OXgloOYglo

OZglo+

10

0cosθ3-sinθ3

0sinθ3cosθ3

KXleg

KYleg

KZleg，（7）

式中：（OXglo， OZglo， OZglo）為側(cè)擺支點(diǎn)O在全局坐標(biāo)系中的坐標(biāo)；（KXleg， KYleg， KZleg）為K點(diǎn)在腿部隨體坐標(biāo)系中的坐標(biāo)。

2? 桿件質(zhì)量離散方法

在進(jìn)行力學(xué)分析時(shí)，腿部機(jī)構(gòu)的自身重力是不可忽略的重要參數(shù)［8］。為了更方便地求解各關(guān)節(jié)的受力情況，利用有限元方法，將腿部機(jī)構(gòu)各桿件的質(zhì)量近似離散到相關(guān)鉸接點(diǎn)［9］。

足式機(jī)器人腿部機(jī)構(gòu)桿件主要有兩節(jié)點(diǎn)線性桿件、三節(jié)點(diǎn)三角形桿件和多節(jié)點(diǎn)線性桿件3種形式。3種桿件的質(zhì)量離散方法分別為線性插值法、面積坐標(biāo)插值法、等質(zhì)量矩分配法。

2.1? 兩節(jié)點(diǎn)線性桿件

將圖3所示的桿件AB視作兩節(jié)點(diǎn)線性有限單元，桿件質(zhì)量離散到鉸接點(diǎn)A的質(zhì)量為

mA=mABlBCmlAB ，（8）

式中：mAB為桿件AB的質(zhì)量；lBCm為B點(diǎn)與桿件質(zhì)心Cm的距離。

桿件質(zhì)量離散到鉸接點(diǎn)B的質(zhì)量為

mB=mABlACmlAB ，（9）

式中l(wèi)ACm為A點(diǎn)與桿件質(zhì)心Cm的距離。

2.2? 三節(jié)點(diǎn)三角形桿件

借鑒有限元分析中三節(jié)點(diǎn)三角形單元的位移場表達(dá)方式即形函數(shù)理論［10］，對(duì)三節(jié)點(diǎn)三角形桿件進(jìn)行質(zhì)量離散。以三節(jié)點(diǎn)三角形桿件BCD為例，如圖4所示，利用桿件鉸接點(diǎn)坐標(biāo)，采用面積坐標(biāo)插值法，將質(zhì)量離散到3個(gè)鉸接點(diǎn)上。同理，此方法適用于三節(jié)點(diǎn)三角形桿件ERQ、IJK。

將三節(jié)點(diǎn)三角形桿件質(zhì)量等效為在質(zhì)心處的集中質(zhì)量，進(jìn)而離散成在節(jié)點(diǎn)上的質(zhì)量，則節(jié)點(diǎn)B、C、D的等效節(jié)點(diǎn)質(zhì)量為

mBmCmD=mBCDNBNCND ，（10）

其中NC=12S（aC+bCCXleg+cCCYleg） ，

aC=DXlegDYleg

BXlegBYleg=DXlegBYleg-BXlegDYleg ，

bC=-1DYleg

1BYleg=DYleg-BYleg ，

cC=1DXleg

1BXleg=BXleg-DXleg ，

（11）

式中：mBCD為三節(jié)點(diǎn)三角形桿件BCD的質(zhì)量；S為BCD的面積。

類似地，將式（11）中B、C、D下標(biāo)輪換，如將下標(biāo)C與D以及C與D的坐標(biāo)互換后進(jìn)行計(jì)算，即可求得NB、ND。

將式（11）計(jì)算得出的（NB， NC， ND）代入式（10），即可得到三節(jié)點(diǎn)三角形桿件質(zhì)量離散到3個(gè)鉸接點(diǎn)的等效質(zhì)量。

2.3? 多節(jié)點(diǎn)線性桿件

圖5所示為腿部機(jī)構(gòu)的多節(jié)點(diǎn)線性桿件結(jié)構(gòu)。例如BRHI桿件，可采用等質(zhì)量矩分配法。

由力矩平衡原理可知， 桿件左、右兩側(cè)鉸接點(diǎn)對(duì)質(zhì)心Cm的總質(zhì)量矩相等，假設(shè)單側(cè)質(zhì)量與距離成反比，求解線性方程組可以得到左、右側(cè)總質(zhì)量矩Mleft、Mright，即

∑1∑1bj

nleft-nright

Mleft

Mright=m0，（12）

式中：aj（i=1，2）、bj（j=1，2，3）分別為左、右側(cè)各鉸接點(diǎn)到質(zhì)心Cm的距離；nleft、nright分別為質(zhì)心的左、右側(cè)節(jié)點(diǎn)個(gè)數(shù)；m為桿件質(zhì)量。

左側(cè)各節(jié)點(diǎn)的等效質(zhì)量為

mi=Mleftai ，（13）

右側(cè)各節(jié)點(diǎn)的等效質(zhì)量為

mj=Mrightbj 。（14）

3? 腿部機(jī)構(gòu)正向與逆向動(dòng)力學(xué)分析

由于足式機(jī)器人腿部運(yùn)動(dòng)過程中結(jié)構(gòu)件加速度遠(yuǎn)小于重力加速度，產(chǎn)生的慣性力顯著小于結(jié)構(gòu)件重力，因此可以忽略慣性力作用。

3.1? 腿部機(jī)構(gòu)逆向動(dòng)力學(xué)分析

為了簡化計(jì)算， 首先利用力矩平衡原理對(duì)腿部機(jī)構(gòu)進(jìn)行逆向動(dòng)力學(xué)分析。 將大腿機(jī)構(gòu)、膝關(guān)節(jié)、小腿機(jī)構(gòu)看作一個(gè)串聯(lián)機(jī)構(gòu)， 基于各鉸接點(diǎn)的力矩平衡， 在以足底力為輸入的情況下， 對(duì)各桿件分別進(jìn)行力學(xué)分析， 計(jì)算大腿、小腿和側(cè)擺油缸的受力情況。 圖6所示為腿部機(jī)構(gòu)整體、桿件ERQ、側(cè)擺油缸的受力分析。 假設(shè)重力加速度g的方向?yàn)樨Q直向下。

3.1.1? 大腿油缸的受力計(jì)算

將除了大腿油缸AC外的其余桿件整體作為研究對(duì)象。利用各點(diǎn)坐標(biāo)及各桿件的長度，求解B點(diǎn)在腿部縱擺平面內(nèi)對(duì)Yleg軸的力矩之和MB，Yleg，根據(jù)力矩平衡條件，求解大腿油缸AC的受力大小F1。在腿部隨體坐標(biāo)系中，B點(diǎn)所受力矩為

MB=BC×F1+BK×FK+∑9i=1（BPi×GPi） ，（15）

令MB，Yleg=0 ，（16）

式中：BC、BK為由B點(diǎn)指向C、K點(diǎn)的矢量；FK為足底力矢量；BPi為由B指向腿部隨體坐標(biāo)系中各鉸接點(diǎn)Pi的矢量，共9個(gè)鉸接點(diǎn)；GPi為腿部機(jī)構(gòu)各鉸接點(diǎn)處的重力矢量；下標(biāo)Yleg代表取向量在腿部隨體坐標(biāo)系中的Yleg分量，以此類推。式（15）中假定由A點(diǎn)指向C點(diǎn)的方向?yàn)镕1的正方向，除了F1大小未知，其余量均已知。

3.1.2? 小腿油缸的受力計(jì)算

以桿件BCD為研究對(duì)象，求解B點(diǎn)在腿部縱擺平面內(nèi)的力矩之和MB，BCD，Yleg，根據(jù)力矩平衡原理，求解桿件ERQ受桿件DE的力的大小FDE。此時(shí)B點(diǎn)所受力矩為

MB，BCD=BC×F1+BD×FDE+BD×GD+BC×GC

，（17）

令MB，BCD，Yleg=0 ，（18）

式中：BD為由B點(diǎn)指向D點(diǎn)的矢量；GC、GD分別為腿部機(jī)構(gòu)桿件重力離散到關(guān)節(jié)鉸接點(diǎn)C、D上的等效矢量。

以桿件IJK為研究對(duì)象，求解I點(diǎn)在腿部縱擺平面內(nèi)的力矩之和MI，Yleg，根據(jù)力矩平衡原理，求解桿件IJK受到桿件JE的力的大小FJE。I點(diǎn)所受力矩為

MI=IK×FK+IJ×FJE+IK×GK+IJ×GJ ，（19）

令MI，Yleg=0 ，（20）

式中：IK、IJ為由I點(diǎn)指向K、J點(diǎn)的矢量；GK、GJ分別為腿部機(jī)構(gòu)桿件重力離散到關(guān)節(jié)鉸接點(diǎn)K、J上的等效矢量。

以桿件ERQ為研究對(duì)象。由受力分析可知R點(diǎn)在腿部縱擺平面內(nèi)的力矩之和MR，Yleg，根據(jù)力矩平衡原理，求解小腿油缸QH的受力大小F2。R點(diǎn)所受力矩為

MR=RE×FDE+RE×FJE+RE×GE+

RQ×F2+RQ×GQ ，（21）

令MR，Yleg=0 ，（22）

式中：RE、RQ為由R點(diǎn)指向E、Q點(diǎn)的矢量；GE、GQ分別為腿部機(jī)構(gòu)桿件重力離散到關(guān)節(jié)鉸接點(diǎn)E、Q上的等效矢量。

3.1.3? 側(cè)擺油缸的受力計(jì)算

在全局坐標(biāo)系中，以整個(gè)腿部為研究對(duì)象，求解髖關(guān)節(jié)側(cè)擺支點(diǎn)O的力矩之和MO，Xglo，根據(jù)側(cè)擺油缸力與足底力和腿部機(jī)構(gòu)重力的力矩平衡原理，求解側(cè)擺油缸O1N的受力大小F3。O點(diǎn)所受力矩為

MO=ON×F3+OK×FK+∑12i=1（OPi×GPi） ，（23）

令MO，Xglo=0 ，（24）

式中， ON、OK、OC為由O點(diǎn)指向N、K、C點(diǎn)的矢量；OPi為由O指向全局坐標(biāo)系中各鉸接點(diǎn)Pi的矢量，共12個(gè)鉸接點(diǎn)。

3.2? 腿部機(jī)構(gòu)正向動(dòng)力學(xué)分析

與逆向動(dòng)力學(xué)分析相似，基于力矩平衡原理，對(duì)腿部機(jī)構(gòu)進(jìn)行正向動(dòng)力學(xué)分析，即利用鉸接點(diǎn)坐標(biāo)、各桿件的重力和油缸力計(jì)算足底力［11］。分別將不同桿件作為研究對(duì)象進(jìn)行力矩平衡分析，具體過程不再贅述，經(jīng)分析得到求解足底力FK的方程為

-（K-I）Zglo（K-I）Xglo

-（K-B）Zglo（K-B）Xglo

0-（K-B）Zglo（K-O）Yglo

FKX

FKY

FKZ=

-

（J-I）XgloFJE，Zglo-（J-I）ZgloFJE，Xglo+（J-I）XgloGJ，Zglo+（K-I）XgloGK，Zglo

-（C-B）XgloFCA，Zglo-（C-B）ZgloFCA，Xglo+（C-B）XgloFC，Zglo+…+（K-B）XgloGK，Zglo

（N-O）Yglo F3，Zglo-（N-O）ZgloF3，Yglo+（C-O）YgloGC，Zglo+…+（K-O）YgloGK，Zglo。（25）

4? 線性變換方程

雖然正向與逆向動(dòng)力學(xué)分析皆基于力矩平衡原理，但是逆向動(dòng)力學(xué)分析的數(shù)學(xué)方程簡潔、直觀，適合采用程序求解。與之相反，正向動(dòng)力學(xué)分析過程復(fù)雜；而且根據(jù)分析思路，正向與逆向動(dòng)力學(xué)分析需要采用不同的數(shù)學(xué)方程，編程時(shí)也需要2個(gè)完全不同的程序，不利于維護(hù)和調(diào)試。

傳遞矩陣法廣泛應(yīng)用于靜力學(xué)與動(dòng)力學(xué)分析，具有建模簡單、使用靈活、計(jì)算效率高的優(yōu)勢(shì)，可以大幅度減小計(jì)算規(guī)模。由于本文中對(duì)腿部機(jī)構(gòu)的正向與逆向動(dòng)力學(xué)分析的數(shù)學(xué)方程可以視為線性方程組，因此借助傳遞矩陣法能夠?qū)⑼炔繖C(jī)構(gòu)整體動(dòng)力學(xué)分析離散為多個(gè)子單元的力學(xué)傳遞問題，推導(dǎo)油缸力與足底力的傳遞矩陣，利用矩陣相乘對(duì)腿部機(jī)構(gòu)進(jìn)行動(dòng)力學(xué)分析，最終建立腿部機(jī)構(gòu)的正-逆向動(dòng)力學(xué)統(tǒng)一模型。

由力平衡方程可知，基于足底力、重力與油缸力的線性關(guān)系，求出足底力傳遞矩陣T，

T=a11a13

a21a23

a31a33 ，（26）

TFK，Xglo

FK，Yglo

FK，Zglo+

F1，g

F2，g

F3，g=F1

F2

F3

，（27）

式中：ai1、ai2、ai3（i=1，2，3）分別為在FK，Xglo、FK，Yglo、FK，Zglo作用下引起的油缸力變化的參數(shù)；（FK，Xglo， FK，Yglo， FK，Zglo）為在整機(jī)全局坐標(biāo)系中的足底接觸力向量；（F1，g， F2，g， F3，g）為只有重力作用下引起的油缸力變化的參數(shù)。

無論是正向動(dòng)力學(xué)分析還是逆向動(dòng)力學(xué)分析，都可以用方程（22）進(jìn)行求解。具體求解過程如下：

步驟1? 確定重力等效貢獻(xiàn)量（F1，g， F2，g， F3，g）

和足底力傳遞矩陣T。

1）取（FK，Xglo， FK，Yglo， FK，Zglo）=（0，0，0），即只考慮重力作用，利用逆向動(dòng)力學(xué)分析式（15）—（24）求解油缸力大小F1、F2、F3，再根據(jù)方程（27）得到重力等效貢獻(xiàn)量（F1，g， F2，g， F3，g）。

2）分別取（FK，Xglo， FK，Yglo， FK，Zglo）=（1，0，0）， （0，1，0）， （0，0，1），此時(shí)不考慮重力作用，利用逆向動(dòng)力學(xué)分析式（15）、（16）、…、（24）求解油缸力大小F1、F2、F3，再代入方程（27），得到由FK，Xglo、FK，Yglo、FK，Zglo引起的油缸力變化參數(shù)，即足底力傳遞矩陣的第1、2、3列的ai1、ai2、ai3。

步驟2? 逆向動(dòng)力學(xué)分析計(jì)算油缸力。將求得的足底力傳遞矩陣T和重力等效貢獻(xiàn)量（F1，g， F2，g， F3，g）代入方程（27），即可求得任意重力和足底力情況時(shí)的油缸力。

步驟3? 正向動(dòng)力學(xué)分析求解計(jì)算足底力。將方程（27）通過變換可以得到

FK，XgloFK，YgloFK，Zglo=T-1

F1F2F3-F1，g

F2，gF3，g

。（28）

將油缸力代入線性方程（28），即可求得該狀態(tài)下的足底力FK。

5? 仿真實(shí)例分析

由于僅驗(yàn)證本文中建立的足式機(jī)器人腿部機(jī)構(gòu)正-逆向動(dòng)力學(xué)統(tǒng)一模型的有效性和可行性， 因此簡化虛擬樣機(jī)建模過程， 省略腿部機(jī)構(gòu)各桿件的具體結(jié)構(gòu)造型， 僅對(duì)桿件長度、運(yùn)動(dòng)副連接、驅(qū)動(dòng)、質(zhì)量參數(shù)等進(jìn)行設(shè)置， 根據(jù)桿件長度與液壓缸初始長度， 計(jì)算得出各鉸接點(diǎn)的初始坐標(biāo)， 利用Adams軟件建立虛擬樣機(jī)模型， 將足底力FK=（20.0， 50.0， 500.0）與初始長度分別為油缸AC長度l1=270 mm、油缸QH長度l2=310 mm、油缸MN長度l3=260 mm的3個(gè)液壓缸長度作為輸入量， 油缸力作為輸出量， 針對(duì)油缸無作動(dòng)與3個(gè)油缸分別作動(dòng)一定量的4種情況進(jìn)行仿真。 腿部機(jī)構(gòu)虛擬樣機(jī)模型如圖7所示。

由于虛擬樣機(jī)模型將桿件質(zhì)量等效在桿件幾何中心，為了減小正-逆向動(dòng)力學(xué)統(tǒng)一模型分析計(jì)算與仿真結(jié)果的誤差，需將實(shí)際質(zhì)心坐標(biāo)賦予虛擬樣機(jī)模型。

逆向動(dòng)力學(xué)仿真結(jié)果如表1所示。由表可知，正-逆向動(dòng)力學(xué)統(tǒng)一模型與虛擬樣機(jī)模型計(jì)算得到的3個(gè)油缸力的誤差均小于1%，驗(yàn)證了本文中腿部機(jī)構(gòu)逆向動(dòng)力學(xué)分析的有效性。

將表1中正-逆向動(dòng)力學(xué)統(tǒng)一模型計(jì)算得出的4種情況下的油缸力與油缸長度作為輸入量， 在虛擬樣機(jī)模型中將油缸力施加于油缸兩端鉸接點(diǎn)處， 足底力作為輸出量， 仿真得到足底力， 結(jié)果如表2所示。 由表可知， 正-逆向動(dòng)力學(xué)統(tǒng)一模型與虛擬樣機(jī)模型計(jì)算得到的3個(gè)方向足底力的誤差均小于3%， 驗(yàn)證了本文中腿部機(jī)構(gòu)正向動(dòng)力學(xué)分析的有效性。

6? 結(jié)論

本文中探討了足式機(jī)器人腿部機(jī)構(gòu)的工作原理，基于力矩平衡原理，研究了腿部機(jī)構(gòu)的油缸力與足底力間的數(shù)學(xué)關(guān)系，得到以下主要結(jié)論：

1）利用有限元方法建立了桿件質(zhì)量離散方法，在動(dòng)力學(xué)分析時(shí)有效考慮腿部機(jī)構(gòu)自重，提高了動(dòng)力學(xué)分析的準(zhǔn)確性。

2）克服了機(jī)器人正向與逆向動(dòng)力學(xué)分析求解過程臃腫的問題， 基于線性變換原理并結(jié)合傳遞矩陣法， 融合正向與逆向動(dòng)力學(xué)分析過程， 有效地簡化了求解過程， 為機(jī)器人的實(shí)時(shí)控制提供了理論基礎(chǔ)。

3）利用Adams軟件建立了虛擬樣機(jī)模型，對(duì)比理論方法與虛擬樣機(jī)模型的輸出量，驗(yàn)證了本文建立的足式機(jī)器人四連桿腿部機(jī)構(gòu)正-逆向動(dòng)力學(xué)統(tǒng)一模型的有效性。

參考文獻(xiàn)：

［1］FOCCHI M， PUCCI D， PRETE A D. Editorial： bridging the gap between the lab and the real world： future perspectives for legged robots［J］. Frontiers in Robotics and AI， 2020， 7： 629002.

［2］崔敏其. SCARA機(jī)器人的拉格朗日動(dòng)力學(xué)建模［J］. 機(jī)械設(shè)計(jì)與制造， 2013（12）： 76.

［3］劉芳華， 馬凡凡， 孫威. 三自由度并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)建模與仿真［J］. 機(jī)床與液壓， 2020， 48（23）： 23.

［4］張鐵， 梁驍翃， 覃彬彬， 等. 基于牛頓-歐拉法的SCARA機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)［J］. 華南理工大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2017， 45（10）： 129.

［5］WU Y， YANG Z， FU Z， et al. Kinematics and dynamics analysis of a novel five-degrees-of-freedom hybrid robot［J］. International Journal of Advanced Robotic Systems， 2017， 14（3）： 12.

［6］王超， 張文輝， 江潔， 等. 基于Matlab/Adams的工業(yè)焊接機(jī)器人運(yùn)動(dòng)學(xué)分析與控制聯(lián)合仿真［J］. 中國工程機(jī)械學(xué)報(bào)， 2020， 18（6）： 504.

［7］LUAN Y Z， GUO J M， LIU H Y. Structural dynamics simulation analysis of industrial robot arm based on Kane method［J］. Journal of Physics： Conference Series， 2021， 1871（1）： 012155.

［8］AMARE Z， ZI B， QIAN S， et al. Dynamic analysis of electrohydrauliccable-drivenparallel robots［J］.ProceedingsoftheInstitution of Mechanical Engineers： Part C： Journal of Mechanical Engineering Science， 2019， 233（10）： 3400.

［9］王勖成， 邵敏. 有限單元法基本原理和數(shù)值方法［M］. 2版.? 北京： 清華大學(xué)出版社， 1997.

［10］曾攀. 有限元分析及應(yīng)用［M］. 北京： 清華大學(xué)出版社， 2004： 144-147.

［11］CHEN G，YUAN J.Nonlineardynamics analysis of shift mani-pulator for robot driver considering multiple revolute clearance joints and variable load［J］. Mechanics Based Design of Structures and Machines， 2020， 49（4）： 596.

（責(zé)任編輯：王? 耘）