多自由度串聯(lián)機(jī)器人的高效率反向動(dòng)力學(xué)建模方法

申浩宇　吳洪濤　陳　柏

南京航空航天大學(xué)，南京，210016

多自由度串聯(lián)機(jī)器人的高效率反向動(dòng)力學(xué)建模方法

申浩宇吳洪濤陳柏

南京航空航天大學(xué)，南京，210016

摘要：針對(duì)多自由度串聯(lián)機(jī)器人的反向動(dòng)力學(xué)建模問題，通過引入解耦的自然正交補(bǔ)的概念和方法，消除了牛頓-歐拉動(dòng)力學(xué)方程中的約束力項(xiàng)，得到了一種基于解耦的自然正交補(bǔ)的高效反向動(dòng)力學(xué)遞推建模方法。編制程序?qū)ζ咦杂啥却?lián)機(jī)器人進(jìn)行了仿真驗(yàn)證，并將其與牛頓-歐拉遞推建模方法進(jìn)行了對(duì)比。結(jié)果表明，基于解耦的自然正交補(bǔ)的反向動(dòng)力學(xué)遞推建模方法可節(jié)省約2/3的CPU計(jì)算時(shí)間，仿真結(jié)果正確、合理，從而驗(yàn)證了該方法的可靠性和高效性。

關(guān)鍵詞：自然正交補(bǔ)；遞推算法；動(dòng)力學(xué)建模；串聯(lián)機(jī)器人

0引言

最近幾十年間，多體系統(tǒng)動(dòng)力學(xué)的應(yīng)用已經(jīng)擴(kuò)展至機(jī)器人、車輛、航空、生物力學(xué)等多個(gè)領(lǐng)域[1-3]。工業(yè)機(jī)器人中的七自由度甚至更多自由度的串聯(lián)機(jī)器人擁有更好的動(dòng)力學(xué)性能，非常適合復(fù)雜的工作場(chǎng)合[4-5]。伴隨著自由度的增加，這類機(jī)器人的動(dòng)力學(xué)建模更加復(fù)雜。因此這種多自由度串聯(lián)機(jī)器人的動(dòng)力學(xué)建模問題一直受到廣泛的關(guān)注[6-8]。

目前常用的動(dòng)力學(xué)建模方法有牛頓-歐拉方法、歐拉-拉格朗日方法、凱恩方法等。但這些方法的計(jì)算效率都不高，均無(wú)法滿足當(dāng)前動(dòng)力學(xué)建模和實(shí)時(shí)仿真的需要[9-12]。為了提高動(dòng)力學(xué)建模的效率，在牛頓-歐拉方法的基礎(chǔ)上，Angeles等[13-14]提出了自然正交補(bǔ)(natural orthogonal complement，NOC)的概念，消除了牛頓-歐拉方程中的約束力項(xiàng)，獲得了最小階數(shù)的動(dòng)力學(xué)約束方程。Saha[15-16]又進(jìn)一步提出了解耦的自然正交補(bǔ)(decoupled NOC，DeNOC)的概念，使動(dòng)力學(xué)模型的物理意義更加明確，同時(shí)也降低了動(dòng)力學(xué)建模的復(fù)雜性。

筆者針對(duì)多自由度串聯(lián)機(jī)器人動(dòng)力學(xué)建模的特點(diǎn)，將有關(guān)解耦的自然正交補(bǔ)的概念和方法應(yīng)用于該多體系統(tǒng)的動(dòng)力學(xué)建模，為多自由度串聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)及動(dòng)力學(xué)高效建模提供了一種有效途徑。最后通過仿真實(shí)驗(yàn)對(duì)該方法的有效性進(jìn)行了驗(yàn)證。

1基于解耦的自然正交補(bǔ)的動(dòng)力學(xué)模型

在非解耦的牛頓-歐拉方程的基礎(chǔ)上，首先利用相鄰構(gòu)件之間的運(yùn)動(dòng)學(xué)約束，得到解耦的自然正交補(bǔ)矩陣，進(jìn)而導(dǎo)出基于解耦的自然正交補(bǔ)的動(dòng)力學(xué)方程。

1.1非解耦的牛頓-歐拉方程

(a)串聯(lián)機(jī)器人

(b)單個(gè)連桿圖1　串聯(lián)機(jī)器人多體系統(tǒng)

(1)

(2)

式(1)和式(2)可以合并寫為6維旋量形式：

(3)

(4)

式中，I3×3、O3×3分別為單位矩陣和零矩陣；mkdk×、ωk×分別為向量mkdk、ωk的叉乘運(yùn)算對(duì)應(yīng)的矩陣算子。

對(duì)于任意3維向量x=(x1，x2，x3)，“x×”表示向量x對(duì)應(yīng)的矩陣算子，即

(5)

串聯(lián)機(jī)器人系統(tǒng)由n個(gè)連桿組成，則整體的非解耦牛頓-歐拉方程為

(6)

M=diag(M1，M2，…，Mn)

Ω=diag(Ω1，Ω2，…，Ωn)

E=diag(E1，E2，…，En)

式中，M為廣義質(zhì)量矩陣，M∈R6n×6n；Ω為加速度張量矩陣，Ω∈R6n×6n；E為與矩陣Mk相對(duì)應(yīng)的耦合矩陣，E∈R6n×6n；t為廣義運(yùn)動(dòng)旋量，t∈R6n×1；w為廣義力旋量，w∈R6n×1。

1.2運(yùn)動(dòng)學(xué)約束

如圖2所示，連桿k和k-1通過旋轉(zhuǎn)關(guān)節(jié)k連接，設(shè)連桿k的坐標(biāo)原點(diǎn)Ok的角速度向量、線速度向量分別為ωk和vk。則兩個(gè)連桿之間的運(yùn)動(dòng)學(xué)約束關(guān)系為

(7)

(8)

圖2　相鄰連桿的運(yùn)動(dòng)學(xué)關(guān)系

通過前面定義，以上2個(gè)運(yùn)動(dòng)學(xué)約束方程(式(7)、式(8))可以寫為以下6維旋量形式：

(9)

其中，Ak,k-1為旋量轉(zhuǎn)化矩陣，Ak,k-1∈R6×6。將連桿k-1的運(yùn)動(dòng)旋量轉(zhuǎn)化至k連桿上，可得

這里，ak,k-1×1為3維向量ak,k-1相應(yīng)的叉乘張量。θk為關(guān)節(jié)變量。pk∈R6×1為連桿k相對(duì)于連桿k-1的相對(duì)速度，該向量的元素主要由關(guān)節(jié)k的類型決定，稱為速度轉(zhuǎn)化向量，對(duì)于旋轉(zhuǎn)關(guān)節(jié)，有

該串聯(lián)機(jī)器人系統(tǒng)整體的運(yùn)動(dòng)學(xué)約束方程可以寫為

(10)

N=NlNd∈R6n×n

(11)

Nd=diag(p1，p2，…，pn)∈R6n×6n

下三角矩陣Nl、分塊對(duì)角矩陣Nd即為串聯(lián)機(jī)器人系統(tǒng)的解耦的自然正交補(bǔ)矩陣[17]。

1.3解耦的牛頓歐拉方程

將非解耦的牛頓-歐拉方程兩邊左乘NT，則式(6)變?yōu)?/p>

(12)

(13)

(14)

式中，I為廣義慣性矩陣；C為慣性傳遞矩陣；τ即為n維廣義力向量。

2基于解耦的自然正交補(bǔ)的反向動(dòng)力學(xué)遞推算法

反向動(dòng)力學(xué)算法可以定義為：給定機(jī)器人的關(guān)節(jié)運(yùn)動(dòng)軌跡，計(jì)算維持運(yùn)動(dòng)所需的驅(qū)動(dòng)力和力矩。由上面的動(dòng)力學(xué)方程(式(14))，以及矩陣I、C、τ的解析表達(dá)式，可以得到以下遞推的反向動(dòng)力學(xué)算法。該算法由兩個(gè)遞推步驟組成。

正向遞推：

t1=p1θ·1t2 =p2θ·2+t1 ?tn=pnθ·n+tn-1

t·1=p1θ¨1+Ω1p1θ·1t·2 =p2θ¨2+Ω2p2θ·2+A21t·1+A·21t1 ?t·n=pnθ¨n+Ωnpnθ·n+An,n-1t·n-1+A·n,n-1tn-1

逆向遞推：

wEn=Mnt·n+ΩnMnEntnwEn-1 =Mn-1t·n-1+Ωn-1Mn-1En-1tn-1+ATn,n-1wEn ?wE1=M1t·1+Ω1M1E1t1+AT21wE2

τn=pTnwEnτn-1 =pTn-1wEn-1 ?τ1=pT1wE1

(15)

3仿真試驗(yàn)和分析

3.1七自由度串聯(lián)機(jī)器人及仿真軌跡

下面將一種串聯(lián)機(jī)器人作為研究實(shí)例(圖3a)，該機(jī)器人為七自由度的串聯(lián)機(jī)械臂(在傳統(tǒng)的六自由度串聯(lián)機(jī)械臂的基礎(chǔ)上增加了一個(gè)冗余自由度)，每個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)。根據(jù)D-H坐標(biāo)系建立原則和各個(gè)桿件的結(jié)構(gòu)，建立運(yùn)動(dòng)學(xué)簡(jiǎn)圖(圖3b)，表1、表2所示為機(jī)器人參數(shù)。

(a)外觀圖(b)D-H坐標(biāo)系圖3　七自由度串聯(lián)機(jī)器人

編號(hào)扭轉(zhuǎn)角αi-1(rad)連桿長(zhǎng)度ai-1(m)連桿偏移量di(m)關(guān)節(jié)角1000.246θ12π/200θ23-π/200.304θ34π/200θ45-π/200.317θ56π/200θ67-π/200.295θ7

表2　七自由度串聯(lián)機(jī)器人慣性參數(shù)

首先，在Mathematics 9.0的軟件環(huán)境下編制程序，運(yùn)用基于解耦正交補(bǔ)的遞推反向動(dòng)力學(xué)建模方法，對(duì)串聯(lián)機(jī)器人系統(tǒng)進(jìn)行動(dòng)力學(xué)建模。然后，基于模型進(jìn)行了動(dòng)力學(xué)仿真，并與傳統(tǒng)的牛頓-歐拉遞推反向動(dòng)力學(xué)建模方法得到的仿真結(jié)果進(jìn)行了對(duì)比。關(guān)節(jié)i(i=1，2，…，7)的仿真運(yùn)動(dòng)軌跡為

(16)

其中，θi(0)、θi(T)分別為初始和最終的關(guān)節(jié)轉(zhuǎn)角。七自由度串聯(lián)機(jī)器人的仿真實(shí)驗(yàn)中，設(shè)置7個(gè)關(guān)節(jié)初始、最終的關(guān)節(jié)轉(zhuǎn)角均為0和π/3，仿真時(shí)間為2s。

3.2結(jié)果分析

在同等計(jì)算條件下(inteli7-3770，內(nèi)存16GB)，基于解耦的自然正交補(bǔ)的反向動(dòng)力學(xué)遞推建模方法的結(jié)果與牛頓-歐拉反向動(dòng)力學(xué)遞推建模方法的結(jié)果完全一致，表明了仿真結(jié)果正確、合理，從而驗(yàn)證了該方法的可靠性和高效性。傳統(tǒng)的牛頓-歐拉反向動(dòng)力學(xué)遞推建模方法的計(jì)算時(shí)間為8.98s，基于解耦的正交補(bǔ)的反向動(dòng)力學(xué)遞推建模方法的計(jì)算時(shí)間為2.86s，較前者節(jié)省了約2/3的CPU計(jì)算時(shí)間。可見，基于解耦的自然正交補(bǔ)的反向動(dòng)力學(xué)遞推建模方法與傳統(tǒng)方法相比明顯具有計(jì)算效率高的優(yōu)點(diǎn)。

在給定式(16)所示的各個(gè)關(guān)節(jié)的運(yùn)動(dòng)軌跡后，通過仿真得到各個(gè)關(guān)節(jié)所需的驅(qū)動(dòng)力矩，如圖4所示。因?yàn)閮煞N建模方法的仿真結(jié)果相同，故只繪制基于解耦的自然正交補(bǔ)的反向動(dòng)力學(xué)遞推建模方法的仿真曲線。圖4中，關(guān)節(jié)1的初始和最終力矩均為0，這是由于關(guān)節(jié)1的運(yùn)動(dòng)不受重力加速度的影響；關(guān)節(jié)2運(yùn)動(dòng)所需要的驅(qū)動(dòng)力矩最大，這是因?yàn)殛P(guān)節(jié)2的運(yùn)動(dòng)需要克服連桿2～7所受重力的影響。

圖4　關(guān)節(jié)驅(qū)動(dòng)力矩的解耦自然正交補(bǔ)仿真結(jié)果

4結(jié)語(yǔ)

針對(duì)多自由度串聯(lián)機(jī)器人的反向動(dòng)力學(xué)建模問題，推導(dǎo)分析了基于解耦正交補(bǔ)的反向動(dòng)力學(xué)遞推建模方法，實(shí)現(xiàn)了基于解耦正交補(bǔ)的多自由度串聯(lián)機(jī)器人的反向動(dòng)力學(xué)高效建模與仿真。

在Mathematic9.0中編制了仿真程序，并將其與牛頓-歐拉反向動(dòng)力學(xué)遞推建模方法的仿真結(jié)果進(jìn)行了對(duì)比。仿真結(jié)果表明，基于解耦的正交補(bǔ)的方法是正確的，且具有編程簡(jiǎn)單、實(shí)用性廣泛、計(jì)算效率高的優(yōu)勢(shì)，提高了反向動(dòng)力學(xué)建模方法的效率。

參考文獻(xiàn)：

[1]熊有倫. 機(jī)器人技術(shù)基礎(chǔ)[M]. 武漢:華中科技大學(xué)出版社, 1996.

[2]吳洪濤，熊有倫. 機(jī)械工程中的多體系統(tǒng)動(dòng)力學(xué)問題[J]. 中國(guó)機(jī)械工程，2000，11(6)：608-611.

WuHongtao，XiongYoulun.TheProblemofMultibodySystemsDynamicsinMechanicalEngineering[J].ChinaMechanicalEngineering，2000，11(6)：608-611.

[3]劉樹青，王興松. 基于零空間法的柔性多體系統(tǒng)動(dòng)力學(xué)計(jì)算及仿真[J]. 東南大學(xué)學(xué)報(bào)(自然科學(xué)版)，2010，40(6)：1348-1352.

LiuShuqing，WangXingsong.DynamicsCalculationandSimulationofFlexibleMulti-bodySystemBasedonNull-spaceMethod[J].JournalofSoutheastUniversity(NaturalScienceEdition)，2010，40(6)：1348-1352.

[4]陸震，何廣平. 冗余自由度機(jī)器人原理及應(yīng)用[M]. 北京：機(jī)械工業(yè)出版社，2007.

[5]孫立寧，趙建文，杜志江. 單冗余度機(jī)器人避障能力指標(biāo)的建立及在7自由度冗余手臂上的實(shí)踐[J]．機(jī)械工程學(xué)報(bào)，2007，43(5)：223-229．

SunLining，ZhaoJianwen，DuZhijiang.ConceptandApplicationon7-DOFRedundantManipulatorofIndextoMeasureCapabilityofCollision-avoidanceforSingleRedundantManipulator[J].ChineseJournalofMechanicalEngineering，2007，43(5)：223-229.

[6]孫奎，金明河，謝宗武，等. 一種穩(wěn)定的冗余度機(jī)器人動(dòng)力學(xué)解法[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2009，41(9)：38-41.

SunKui，JingMinghe，XieZongwu，etal.AStabilizedDynamicsResolutionforRedundantManipulators[J].JournalofHarbinInstituteofTechnology，2009，41(9)：38-41.

[7]陳偉海，游賢強(qiáng)，崔翔，等. 繩驅(qū)動(dòng)擬人臂機(jī)器人的動(dòng)力學(xué)建模及張力分析[J]. 北京航空航天大學(xué)學(xué)報(bào)，2013，39(3)：335-339.

ChenWeihai，YouXianqiang，CuiXiang，etal.DynamicsModelingandTensionAnalysisforaCable-drivenHumanoid-armRobot[J].JournalofBeijingUniversityofAeronauticsandAstronautics，2013，39(3)：335-339.

[8]AgarwalA，ShahSV，BandyopadhyayS，etal.DynamicsofSerialKinematicChainswithLargeNumberofDegrees-of-freedom[J].MultibodySystemDynamics，2014, 32(3): 273-298.

[9]方喜峰，吳洪濤，劉云平，等. 基于空間算子代數(shù)理論計(jì)算多體系統(tǒng)動(dòng)力學(xué)建模[J]. 機(jī)械工程學(xué)報(bào)，2009，45(1)：228-234.

FangXifeng，WuHongtao，LiuYunping，etal.ComputationalDynamicsofMulti-bodySystemModelingBasedonSpatialOperatorAlgebraTheory[J].JournalofMechanicalEngineering，2009，45(1)：228-234.

[10]MohanA，SahaSK.ARecursive,NumericallyStable,andEfficientSimulationAlgorithmforSerialRobotswithFlexibleLinks[J].MultibodySystemDynamics，2009，21(1)：1-35.

[11]邵兵，吳洪濤，程世利，等. 基于李群李代數(shù)的主被動(dòng)關(guān)節(jié)機(jī)器人動(dòng)力學(xué)及控制[J]. 中國(guó)機(jī)械工程，2010，21(3)：253-257.

ShaoBing，WuHongtao，ChengShili，etal.DynamicsandControlofRobotwithActiveandPassiveJointsUsingLieGroupsandLieAlgebras[J].ChinaMechanicalEngineering，2010，21(3)：253-257.

[12]黃玉釧，曲道奎，徐方. 基于自然正交補(bǔ)的真空機(jī)器人動(dòng)力學(xué)建模[J]. 機(jī)器人，2012，34(6)：730-736.

HuangYuchuan，QuDaokui，XuFang.DynamicModelingofVacuumRobotsBasedonNaturalOrthogonalComplement[J].Robot，2012，34(6)：730-736.

[13]AngelesJ，LeeSK.TheFormulationofDynamicalEquationsofHolonomicMechanicalSystemsUsingaNaturalOrthogonalComplement[J].JournalofAppliedMechanics，1988，55(1)：243-244.

[14]AngelesJ. 機(jī)器人機(jī)械系統(tǒng)原理: 理論、方法和算法[M]. 宋偉剛，譯. 北京：機(jī)械工業(yè)出版社，2004.

[15]SahaSK.DynamicsofSerialMultibodySystemsUsingtheDecoupledNaturalOrthogonalComplementMatrices[J].JournalofAppliedMechanics，1999，66(4)：986-996.

[16]SahaSK.RecursiveDynamicsAlgorithmsforSerial,Parallel,andClosed-chainMultibodySystems[C]//Indo-USWorkshoponProteinKinematics&ProteinConformations.Bangalore，2007：10-11.

[17]ShahSV，SahaSK，DuttJK.DynamicsofTree-typeRoboticSystems[M].Dordrecht:Springer, 2013.

(編輯張洋)

High Effective Inverse Dynamics Modelling for Multi-DOF Serial Robots

Shen HaoyuWu HongtaoChen Bai

Nanjing University of Aeronautics and Astronautics，Nanjing，210016

Abstract:To deal with the problem of inverse dynamics modelling for multi-DOF serial robot, a recursive inverse dynamics modelling method was presented based on decoupled natural orthogonal complement. In this model, the concepts and methods of decoupled natural orthogonal complement(DeNOC) matrices were used to eliminate the constraint forces in the Newton-Euler kinematic equations. Finally, a 7-DOF serial robot was simulated by, and the results were compared with those of the recursive Newton-Euler kinematic equations. Simulation results show that the proposed method based on decoupled natural orthogonal complement can save 2/3 amount of CPU time that is spent in computing compared with the traditional approach, and the results is correct and reasonable, which can verify the reliability and efficiency of the method.

Key words:natural orthogonal complement; recursive algorithm; dynamics modeling; serial robot

收稿日期：2015-03-27

基金項(xiàng)目：國(guó)家自然科學(xué)基金資助項(xiàng)目(51375230)；國(guó)家高技術(shù)研究發(fā)展計(jì)劃(863計(jì)劃)資助項(xiàng)目(2013AA041004)；江蘇省科技支撐計(jì)劃資助重點(diǎn)項(xiàng)目(BE2013003-1，BE2013010-2)

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

DOI：10.3969/j.issn.1004-132X.2016.01.004

作者簡(jiǎn)介：申浩宇，男，1986年生。南京航空航天大學(xué)機(jī)電學(xué)院博士研究生。主要研究方向?yàn)闄C(jī)器人動(dòng)力學(xué)及控制。發(fā)表4篇論文。吳洪濤(通信作者)，男，1962年生。南京航空航天大學(xué)機(jī)電學(xué)院教授、博士研究生導(dǎo)師。陳柏，男，1978年生。南京航空航天大學(xué)機(jī)電學(xué)院教授、博士研究生導(dǎo)師。