多自由度串聯機器人的高效率反向動力學建模方法

申浩宇　吳洪濤　陳　柏

南京航空航天大學，南京，210016

多自由度串聯機器人的高效率反向動力學建模方法

申浩宇吳洪濤陳柏

南京航空航天大學，南京，210016

摘要：針對多自由度串聯機器人的反向動力學建模問題，通過引入解耦的自然正交補的概念和方法，消除了牛頓-歐拉動力學方程中的約束力項，得到了一種基于解耦的自然正交補的高效反向動力學遞推建模方法。編制程序對七自由度串聯機器人進行了仿真驗證，并將其與牛頓-歐拉遞推建模方法進行了對比。結果表明，基于解耦的自然正交補的反向動力學遞推建模方法可節省約2/3的CPU計算時間，仿真結果正確、合理，從而驗證了該方法的可靠性和高效性。

關鍵詞：自然正交補；遞推算法；動力學建模；串聯機器人

0引言

最近幾十年間，多體系統動力學的應用已經擴展至機器人、車輛、航空、生物力學等多個領域[1-3]。工業機器人中的七自由度甚至更多自由度的串聯機器人擁有更好的動力學性能，非常適合復雜的工作場合[4-5]。伴隨著自由度的增加，這類機器人的動力學建模更加復雜。因此這種多自由度串聯機器人的動力學建模問題一直受到廣泛的關注[6-8]。

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

筆者針對多自由度串聯機器人動力學建模的特點，將有關解耦的自然正交補的概念和方法應用于該多體系統的動力學建模，為多自由度串聯機器人的運動學及動力學高效建模提供了一種有效途徑。最后通過仿真實驗對該方法的有效性進行了驗證。

1基于解耦的自然正交補的動力學模型

在非解耦的牛頓-歐拉方程的基礎上，首先利用相鄰構件之間的運動學約束，得到解耦的自然正交補矩陣，進而導出基于解耦的自然正交補的動力學方程。

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

(a)串聯機器人

(b)單個連桿圖1　串聯機器人多體系統

(1)

(2)

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

(3)

(4)

式中，I3×3、O3×3分別為單位矩陣和零矩陣；mkdk×、ωk×分別為向量mkdk、ωk的叉乘運算對應的矩陣算子。

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

(5)

串聯機器人系統由n個連桿組成，則整體的非解耦牛頓-歐拉方程為

(6)

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

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

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

式中，M為廣義質量矩陣，M∈R6n×6n；Ω為加速度張量矩陣，Ω∈R6n×6n；E為與矩陣Mk相對應的耦合矩陣，E∈R6n×6n；t為廣義運動旋量，t∈R6n×1；w為廣義力旋量，w∈R6n×1。

1.2運動學約束

如圖2所示，連桿k和k-1通過旋轉關節k連接，設連桿k的坐標原點Ok的角速度向量、線速度向量分別為ωk和vk。則兩個連桿之間的運動學約束關系為

(7)

(8)

圖2　相鄰連桿的運動學關系

通過前面定義，以上2個運動學約束方程(式(7)、式(8))可以寫為以下6維旋量形式：

(9)

其中，Ak,k-1為旋量轉化矩陣，Ak,k-1∈R6×6。將連桿k-1的運動旋量轉化至k連桿上，可得

這里，ak,k-1×1為3維向量ak,k-1相應的叉乘張量。θk為關節變量。pk∈R6×1為連桿k相對于連桿k-1的相對速度，該向量的元素主要由關節k的類型決定，稱為速度轉化向量，對于旋轉關節，有

該串聯機器人系統整體的運動學約束方程可以寫為

(10)

N=NlNd∈R6n×n

(11)

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

下三角矩陣Nl、分塊對角矩陣Nd即為串聯機器人系統的解耦的自然正交補矩陣[17]。

1.3解耦的牛頓歐拉方程

將非解耦的牛頓-歐拉方程兩邊左乘NT，則式(6)變為

(12)

(13)

(14)

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

2基于解耦的自然正交補的反向動力學遞推算法

反向動力學算法可以定義為：給定機器人的關節運動軌跡，計算維持運動所需的驅動力和力矩。由上面的動力學方程(式(14))，以及矩陣I、C、τ的解析表達式，可以得到以下遞推的反向動力學算法。該算法由兩個遞推步驟組成。

正向遞推：

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仿真試驗和分析

3.1七自由度串聯機器人及仿真軌跡

下面將一種串聯機器人作為研究實例(圖3a)，該機器人為七自由度的串聯機械臂(在傳統的六自由度串聯機械臂的基礎上增加了一個冗余自由度)，每個關節均為旋轉關節。根據D-H坐標系建立原則和各個桿件的結構，建立運動學簡圖(圖3b)，表1、表2所示為機器人參數。

(a)外觀圖(b)D-H坐標系圖3　七自由度串聯機器人

編號扭轉角αi-1(rad)連桿長度ai-1(m)連桿偏移量di(m)關節角1000.246θ12π/200θ23-π/200.304θ34π/200θ45-π/200.317θ56π/200θ67-π/200.295θ7

表2　七自由度串聯機器人慣性參數

首先，在Mathematics 9.0的軟件環境下編制程序，運用基于解耦正交補的遞推反向動力學建模方法，對串聯機器人系統進行動力學建模。然后，基于模型進行了動力學仿真，并與傳統的牛頓-歐拉遞推反向動力學建模方法得到的仿真結果進行了對比。關節i(i=1，2，…，7)的仿真運動軌跡為

(16)

其中，θi(0)、θi(T)分別為初始和最終的關節轉角。七自由度串聯機器人的仿真實驗中，設置7個關節初始、最終的關節轉角均為0和π/3，仿真時間為2s。

3.2結果分析

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

在給定式(16)所示的各個關節的運動軌跡后，通過仿真得到各個關節所需的驅動力矩，如圖4所示。因為兩種建模方法的仿真結果相同，故只繪制基于解耦的自然正交補的反向動力學遞推建模方法的仿真曲線。圖4中，關節1的初始和最終力矩均為0，這是由于關節1的運動不受重力加速度的影響；關節2運動所需要的驅動力矩最大，這是因為關節2的運動需要克服連桿2～7所受重力的影響。

圖4　關節驅動力矩的解耦自然正交補仿真結果

4結語

針對多自由度串聯機器人的反向動力學建模問題，推導分析了基于解耦正交補的反向動力學遞推建模方法，實現了基于解耦正交補的多自由度串聯機器人的反向動力學高效建模與仿真。

在Mathematic9.0中編制了仿真程序，并將其與牛頓-歐拉反向動力學遞推建模方法的仿真結果進行了對比。仿真結果表明，基于解耦的正交補的方法是正確的，且具有編程簡單、實用性廣泛、計算效率高的優勢，提高了反向動力學建模方法的效率。

參考文獻：

[1]熊有倫. 機器人技術基礎[M]. 武漢:華中科技大學出版社, 1996.

[2]吳洪濤，熊有倫. 機械工程中的多體系統動力學問題[J]. 中國機械工程，2000，11(6)：608-611.

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

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

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

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

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

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

[6]孫奎，金明河，謝宗武，等. 一種穩定的冗余度機器人動力學解法[J]. 哈爾濱工業大學學報，2009，41(9)：38-41.

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

[7]陳偉海，游賢強，崔翔，等. 繩驅動擬人臂機器人的動力學建模及張力分析[J]. 北京航空航天大學學報，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]方喜峰，吳洪濤，劉云平，等. 基于空間算子代數理論計算多體系統動力學建模[J]. 機械工程學報，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]邵兵，吳洪濤，程世利，等. 基于李群李代數的主被動關節機器人動力學及控制[J]. 中國機械工程，2010，21(3)：253-257.

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

[12]黃玉釧，曲道奎，徐方. 基于自然正交補的真空機器人動力學建模[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. 機器人機械系統原理: 理論、方法和算法[M]. 宋偉剛，譯. 北京：機械工業出版社，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

基金項目：國家自然科學基金資助項目(51375230)；國家高技術研究發展計劃(863計劃)資助項目(2013AA041004)；江蘇省科技支撐計劃資助重點項目(BE2013003-1，BE2013010-2)

中圖分類號：TP242

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

作者簡介：申浩宇，男，1986年生。南京航空航天大學機電學院博士研究生。主要研究方向為機器人動力學及控制。發表4篇論文。吳洪濤(通信作者)，男，1962年生。南京航空航天大學機電學院教授、博士研究生導師。陳柏，男，1978年生。南京航空航天大學機電學院教授、博士研究生導師。