一種3自由度并聯擬人機械腿的動力學建模及伺服電機峰值力矩預估

李研彪，李景敏，計時鳴，鄭超，趙章風

(浙江工業大學 機械工程學院，浙江 杭州310014)

0 引言

目前，多數機械腿采用串聯機構，具有結構復雜、承載能力小、運動慣性大等不足之處［1］，如日本早稻田大學研制的WL 系列雙足擬人機器人，其雙腿均采用串聯機構，各腿具有5 個自由度。另外，日本本田公司、美國Sarcos 研究公司、德國Karlsruhe大學和北京航空航天大學等國內外機器人研究機構也相繼研制出多種串聯結構的仿生機械腿。相對串聯機構，并聯機構具有結構緊湊、承載能力大等優點［2］。本文對一種并聯擬人機械腿的動力學模型及伺服電機峰值力矩預估進行了研究，其機構原型采用并聯機構，如圖1(a)所示。這種擬人機械腿通過3 個電機驅動，實現膝關節和踝關節的運動，類似于人體膝關節和踝關節的結構特點。其驅動裝置安放在固定件的位置上，該機構的運動桿件不必承載驅動電機的重量，減少了電機數量，這種擬人機械腿具有結構簡單、承載能力強、運動慣性小、運動靈活等優點，避免了傳動系統復雜、動態特性差等缺點［1，3］。這種擬人機械腿與髖關節(髖關節機構原型為具有2 轉動自由度的球面并聯機構)串聯在一起，構成了擬人下肢機器人，如圖1(b)和圖1(c)所示。

動力學研究的是物體的運動和作用力之間的關系。機器人的動力學模型用于描繪機器人這種復雜動力系統，以處理其動力響應、動力仿真和計算控制等［4］。目前研究機器人系統動力學的方法很多，有拉格朗日方法［5-11］、牛頓-歐拉方法、高斯方法、凱恩方法、旋量(對偶數)方法［12］、羅伯遜-魏登堡方法和影響系數方法等，其中拉格朗日方法和牛頓-歐拉方法運用較多。拉格朗日方法不僅能以最簡單的形式求得非常復雜的系統動力學方程，而且具有顯式結構。牛頓-歐拉方法則是基于運動坐標系和達朗貝爾原理建立起來的，沒有多余信息，計算速度快。從事這方面理論研究的學者比較具代表性的有Lee 等［13］、Cheroutre-Vialette 等［14］和Miller 等［15］。目前，對機構的驅動端伺服電機峰值力預估的研究已經取得一些研究成果，為本研究的實施奠定了基礎［16-19］。本文在運動學基礎上采用拉格朗日方法對該并聯機械腿進行動力學分析，并在動力學模型的基礎上對驅動端伺服電機模型進行預估，得到了該機械腿的動力學特性。機械腿的研究是步行機器人研究的核心內容，其性能決定了整個機器人的性能。所以該機械腿的研究為這種并聯擬人機械腿的應用奠定了理論基礎。

圖1 擬人下肢機器人Fig.1 Mechanical legs

1 機構簡述

這種擬人機械腿機構以一種3 自由度并聯機構為機構原型，如圖1(a)所示。該機械腿機構采用2PUU(移動副-胡克鉸-胡克鉸)/PRRRU(移動副-轉到副-轉到副-轉到副-胡克鉸)的結構形式，動平臺通過2 條相同的支鏈PUU 和一條支鏈PRRRU 與靜平臺相連。其結構布局特點如下:

1)3 個直線移動副P1、P2和P3相互平行且均安裝在基座上。

2)當桿FJ 與桿IF 共線，面JMN 垂直于桿FJ時，各移動副輸入均為0，這種狀態為此機構的初始位姿。此時，移動副P1、P2、和P3的初始長度為li，i=1，2，3，Δli為直線移動副P1、P2、和P3的輸入位移。根據這種機構的結構特點可知:該機械腿可繞點F 轉動以實現膝關節運動，可繞J 點的Xp軸和Yp軸轉動來實現踝關節的運動。建立2 個坐標系{B}:ObXbYbZb和{P}:OpXpYpZp，其中坐標系{B}為基坐標系，坐標系{P}為動坐標系。

2 運動反解

設:v=(vxvyvz)T為動平臺上的參考點Op在基坐標系{B}(以下無特殊說明均表示在該坐標系下定義)中的速度矢量;ω=(ωxωyωz)T為動平臺的角速度矢量;vi分別為移動副Pi，i =1，2，3的輸入速度;vD表示鉸鏈點D 的速度矢量;vE表示鉸鏈點E 的速度矢量;vG表示鉸鏈點G 的速度矢量;vH表示鉸鏈H 點的速度矢量。則

式中:α、β 分別為動坐標系統固定坐標系的Yb軸、Xb軸轉過的角度。

設rDE為鉸鏈點D 到鉸鏈點E 的矢量，可得

已知

式中:Eb、Db分別為基坐標系下E 點、D 點的位置矢量。由(1)式、(2)式、(3)式可得

設:rJM、rGM、rJN、rHN分別為鉸鏈點J 到鉸鏈點M的矢量，鉸鏈點G 到鉸鏈點M 的矢量，鉸鏈點J 到鉸鏈點N 的矢量，鉸鏈點H 到鉸鏈點M 的矢量，vM表示鉸鏈點M 的速度矢量，vN表示鉸鏈點N 的速度矢量，可得

已知

式中:Mb、Nb、Opb、Gb、Hb分別為基坐標系下各點的位置矢量。

由(1)式～(7)式可得

式中:

把(4)式、(8)式、(9)式寫成統一的形式為

3 動力學建模

在運動學的基礎上已求得各個點的位置矢量，動坐標系{P}在基坐標系{B}下的轉換矩陣T 以及線速度雅可比矩陣Jv和角速度雅克比矩陣Jω. 根據拉格朗日方程，有

式中:L=P -U 表示拉格朗日函數，P 是系統的動能，U 是系統的勢能;qj為廣義坐標，qj=(z1z2z3)T，z1、z2、z3分別為移動副P1、P2、P3在Zb方向的位置矢量;為廣義坐標的一階導數，即為對應于廣義坐標的廣義力。

設3 個移動副Pi，i =1，2，3 的質量分別為m1、m2、m3，速度分別為v1、v2、v3，則3 個移動副的總動能為

所以移動副總動能的廣義坐標表達為

式中:Da=diag(［m1m2m3］);va=設從動桿DE、EF、FOp、GM、HN 的質量分別為mDE、mEF、mFOp、mGM、mHN，由機械腿結構布局特點可知

則鉸鏈點Op的速度

式中:rFOp為鉸鏈點到鉸鏈點Op的矢量;ωF為桿EFJ繞鉸鏈F 點轉動的角速度，并用vF表示鉸鏈F 點的速度矢量。則

桿FOp的動能為

式中:IFOp為桿FOp的轉動慣量;

桿EF 與桿FOp的轉動速度相等，桿EF 的動能為

式中:IEF為桿EF 的轉動慣量;

式中:rDE、rFE分別為鉸鏈點D 到鉸鏈點E 的矢量和鉸鏈點F 到鉸鏈點E 的矢量。

設鉸鏈點E 的速度矢量為vE，桿DE 繞點D 的轉動速度為ωD，則

把(17)式、(20)式代入(21)式整理得

設DE 桿質心的速度矢量為vDE，則

式中:BDE=桿DE 的動能為

式中:DDE=BTDEMDEBDE，MDE為桿DE 的質量矩陣，MDE=diag(［mDEmDEmDE］).

vM和vN表示鉸鏈M 和N 點的速度矢量，rJM和rJN分別為鉸鏈點J 到鉸鏈點M 和N 的矢量，以vG和vH表示鉸鏈G 和H 點的速度矢量，rGM和rHN分別為鉸鏈點G 到點M 和鉸鏈點H 到點N 的矢量，ωG、ωH、ω 分別為桿GM、HN 和動平臺的轉動速度，有

由(25)式得

式中:JM=

Ti，j為姿態轉換矩陣T 的第i 行、第j 列，i、j=1，2，3.則GM 桿質心點的速度為

桿GM 的動能為

式中:DGM=JTGMMGMJGM，MGM為桿GM 的質量矩陣，MGM=diag(［mGMmGMmGM］).

由(29)式得

式中:JN=

則HN 桿質心點的速度為

桿HN 的動能為

式中:DHN= JTHNMHNJHN，MHN為桿HN 的質量矩陣，MHN=diag(［mHNmHNmHN］).

設動平臺的質量為md，Id為運動平臺相對于固定坐標系{B}的慣量矩陣，質心的速度矢量為

已知速度雅克比矩陣為

則動平臺質心點的速度vd為

式中:rd為在動坐標系{P}下點Op到動平臺質心點的矢量。

由(33)式、(34)式可得

式中:Jd=

則動平臺的動能為

由(12)式、(18)式、(19)式、(24)式、(28)式、(32)式和(36)式可得系統總的動能為

式中:D=Da+DFJ+DEF+DDE+DGM+DHN+Dd.

取ObXbYb面為零勢能面，則系統的勢能為0.

將系統總動能P 代入拉格朗日方程得

首先對廣義坐標z1進行運算，則

式中:d1=(1 0 0)，為對廣義坐標的偏導［20-21］。

令M1=d1D，同理可求M2=d2D，M3=d3D，其中d2=(0 1 0)，d3= (0 0 1)，分別為=對和的偏導數。則機械腿慣性矩陣為

將該機構的動力學方程寫成如下一般形式:

4 伺服電機峰值預估模型

4.1 伺服電機轉速預估

驅動3 個移動副的3 個伺服電機分別通過聯軸器與絲杠連接，移動副與絲杠螺母固定聯接，如圖2所示。電機的轉動帶動絲杠的轉動，絲杠只轉動不移動，與絲杠配合的絲杠螺母產生直線位移，從而帶動移動副移動，最終實現膝關節和踝關節的運動形式。

圖2 伺服電機驅動結構圖Fig.2 Driving structure of servo motor

選用單線螺紋絲杠，材料為45 號鋼，螺距p =1.25 mm，公稱直徑d =8 mm，螺紋為梯形粗牙普通螺紋，牙形角λ=30°.

前面已求出3 個直線移動副的移動速度與膝關節和踝關節轉動角度的關系，移動副的運動是由電機轉動通過絲杠傳動傳遞過來的，因為移動副的運動方式是直線運動，所以要求電機的轉動速度需要根據絲杠傳遞運動的特點把線速度轉換成角速度。

設3 個電機的轉動角速度分別為ω1、ω2和ω3，分別對應于移動副P1、P2、P3. 易得電機的轉動速度等于絲杠的轉動速度，由絲杠傳遞運動的特點可得

所以

(45)式即為電機轉動速度與膝關節和踝關節轉動角度之間的關系式。

4.2 伺服電機轉矩預估

由(43)式，伺服電機驅動力的表達式為

由運動反解可得

考慮到伺服電機快速啟動的情況，可以忽略(43)式中的哥氏力和離心力項，伺服電機的峰值力為

則

由于M(q)都隨姿態變化，因此取它們的全域最大值來抵抗哥氏力和離心力項引起的負載力矩。因此，伺服電機峰值力預估模型為

計算可得3 個電機的最大驅動力

在(48)式中，左邊的f 為廣義力，廣義坐標選取不同，則廣義力所代表的含義不同。如果選取動平臺的轉動角度作為廣義坐標，那么根據動力學模型所求得的廣義力不是驅動力，還要乘以一個關節空間與操作空間的力雅克比矩陣。對于該機械腿機構，選取3 個移動副的輸入位移作為廣義坐標，那么廣義力f 的大小就是電機驅動力的大小，不用再求解力雅克比矩陣。在(48)式中，已知的是α、β、θ 的運動規律，在運動反解中已求得α、β、θ 與廣義坐標的關系，所以可很容易地求出廣義力f 的大小。

因為電機是通過絲杠把電機的轉矩轉換成移動副的驅動力，所以要把移動副所受的驅動力f 轉換成轉矩，力與力矩之間的轉換關系式為

式中:Q 表示轉矩;ρ 為絲杠螺紋表面的摩擦角。

本文選取的絲杠螺紋為梯形螺紋，所以牙形角λ=30°，與螺紋的升角相等。絲杠的材質選擇45 號鋼調質處理，絲杠傳動選擇動摩擦，有潤滑劑，摩擦系數取值范圍為0.05 ～0.10，這里取0.08，所以摩擦角為

由(52)式、(53)式得

由(51)式、(54)式可得電機峰值預估最大驅動力矩

5 實例分析與仿真

機械腿所采用參數如下:a =150 mm，b =300 mm，c=150 mm，d=440 mm，e =440 mm，f =300 mm，h =300 mm，k = 320 mm，m = 440 mm，n = 180 mm，l1=200 mm，l2=440 mm，l3=440 mm，φ=120°.

機械腿的3 個轉動角θ、α 和β 的運動規律為

這里取絲杠螺紋為單線，借助于MATLAB 進行仿真，伺服電機的轉速變化規律和轉矩變化規律分別如圖3和圖4所示。

圖3 3 個伺服電機轉速變化曲線Fig.3 Speed change curves of 3 servo motors

從仿真結果可看出，3 個伺服電機的轉速和轉矩均為周期性變化，并且電機最大力矩小于電機峰值預估最大力矩。伺服電機轉速的變化與膝關節和踝關節轉動角度有關，也與絲杠的線數有關;驅動力矩的大小與絲杠的選取有關，比如絲杠螺紋的線數，絲杠的材質，有無潤滑等等，它還與給定的膝關節和踝關節的轉動角度有關。

6 結論

1)對一種并聯擬人機械腿機構進行運動反解分析，給出了輸入速度與輸出速度之間的映射關系。

2)利用拉格朗日方法建立了該機構的動力學模型，并基于該模型建立伺服電機的預估模型。

3)對伺服電機預估模型進行仿真分析，仿真結果表明電機速度和轉矩均為周期性變化，為電機選型奠定理論基礎。

圖4 3 個伺服電機轉矩變化曲線Fig.4 Torque change curves of 3 servo motors

References)

［1］ 李研彪，劉毅，計時鳴，等. 新型擬人機械腿的參數優化［J］.中國機械工程，2013，24(9):1239 -1245.LI Yan-biao，LIU Yi，JI Shi-ming，et al. Parameter optimization for a novel mechanical leg［J］. China Mechanical Engineering，2013，24(9):1239 -1245.(in Chinese)

［2］ 李曉冬. 并聯機器人的特點及應用分析［J］. 黑龍江科技信息，2008，1(5):36 -37.LI Xiao-dong. The characteristics and application analysis of parallel robot［J］. Heilongjiang Science and Technology Information，2008，1(5):36 -37.(in Chinese)

［3］ 李研彪，劉毅，趙章風，等. 基于空間模型技術的擬人機械腿的運動學傳遞性能分析［J］. 農業工程學報，2013，29(2):17 -23.LI Yan-biao，LIU Yi，ZHAO Zhang-feng，et al. Kinematics transmission analysis on anthropopathic mechanical leg based on spatial model technique［J］. Transactions of the Chinese Society of Agricultural Engineering，2013，29(2):17 -23.(in Chinese)

［4］ 黃真，趙永生，趙鐵石. 高等空間機構學［M］. 北京:高等教育出版社，2006.HUANG Zhen，ZHAO Yong-sheng，ZHAO Tie-shi.Advanced spatial mechanism. ［M］. Beijing:Higher Education Press，2006.(in Chinese)

［5］ Gao F，Wang G F，Cui Y，et al. Dynamic modeling and analysis of 9-DOF omnidirectional legged vehicle［J］. Chinese Journal of Mechanical Engineering，2011，24(4):515 -521.

［6］ 張典范. 新型6-(P-2P-S)并聯機器人動力學建模及其運動控制研究［D］. 秦皇島:燕山大學，2009:39 -46.ZHANG Dian-fan. Dynamics modeling and motion control analysis of noval 6-(P-2P-S)parallel robot［D］. Qinhuangdao:Yanshan University，2009:39 -46.(in Chinese)

［7］ Rim B，Habib N，Hala B，et al. Dynamic modeling and control of a multi-fingered robot hand for grasping task［J］. Procedia Engineering，2012，41:923 -931.

［8］ Amira A，Olfa B. A relevant reduction method for dynamic modeling of a seven-linked humanoid robot in the three-dimensional space［J］. Procedia Engineering，2012，41:1277 -1284.

［9］ Arsenault M，Gosselin C M，Saravanan S. Kinematic，static and dynamic analysis of a planar 2-DOF tensegrity mechanism［J］.Mechanism and Machine Theory，2006，41(9):1072 -1089.

［10］ Bai Z F，Han X G，Chen W Y. Study of a 3-DOF parallel manipulator dynamics based on Lagrange equation［J］. Journal of Beijing University of Aeronautics and Astronautics，2004，30(1):51 -54.

［11］ Rong F F，Ying S K，Guo C W. Dynamic analysis and system identification of an LCD glass-handling robot driven by a PMSM［J］.Applied Mathematical Medelling，2009，34(5):1360 -1381.

［12］ 王紅旗，王泰華. 基于旋量理論的移動機械手動力學建模［J］.兵工學報，2010，31(6):849 -854.WANG Hong-qi，WANG Tai-hua. Dynamic modeling of mobile manipulator based on scerw theory［J］. Acta Armamentarii，2010，31(6):849 -854.(in Chinese)

［13］ Lee J D，Geng Z. A dynamic model of a flexible Stewart platform［J］.Computers and Structures，1993，48(3):367 -374.

［14］ Cheroutre-Vialette M，Lebert A. Growth of Listeria monocytogenes as a function of dynamic environment at 10 ℃and accuracy of growth predictions with available models［J］. Food Microbiology，2000，17(1):83 -92.

［15］ Miller K. Experimental verification of modeling of DELTA robot dynamics by direct application of Hamilton's principle［C］∥Proceedings of 1995 IEEE International Conference on Robotics and Automation. Nagoya-shi，Japan:IEEE，1995:532 -537.

［16］ 謝慶華，張琦，楊小川. 液壓排雷機械手作業系統的運動學分析［J］. 兵工學報，2004，25(5):572 -575.XIE Qing-hua，ZHANG Qi，YANG Xiao-chuan. Kinematic analysis of mine clearing manipulator operation of the hydraulic system［J］. Acta Armamentarii，2004，25(5):572 - 575. (in Chinese)

［17］ 金振林，崔冰艷. 機器人肩關節的動力學建模及伺服電機峰值預估［J］. 農業工程學報，2011，27(8):145 -149.JIN Zhen-lin，CUI Bing-yan. Dynamic modeling and peak prediction of servo motor for shoulder joint of robot［J］. Transactions of the Chinese Society of Agricultural Engineering，2011，27(8):145 -149. (in Chinese)

［18］ 榮譽，金振林. 3-DOF 并聯機械腿動力學建模與伺服電機峰值預估［J］. 光學精密工程，2012，20(9):1974 -1983.RONG Yu，JIN Zhen-lin. Dynamic modeling of 3-DOF parallel mechanical leg and peak prediction of servo motor［J］. Optics and Precision Engineering，2012，20(9):1974-1983.(in Chinese)

［19］ 李金泉，楊向東，付鐵. 碼垛機器人機械結構與控制系統設計［M］. 北京:北京理工大學出版社，2011.LI Jin-quan，YANG Xiang-dong，FU Tie. Mechanical structure and control design of palletizing robot［M］.Beijing:Beijing Institute of Technology Press，2011.(in Chinese)

［20］ 王躍靈. 擬人機械臂動力學建模與智能控制研究［D］. 秦皇島:燕山大學，2010.WANG Yue-ling. Dynamics modeling and intelligent control for humanoid mechanical arm［D］. Qinhuangdao:Yanshan University，2010.(in Chinese)

［21］ 施毅. 新型五自由度并聯機床的若干基礎問題及動力學分析［D］. 秦皇島:燕山大學，2005.SHI Yi. Some foundation issues and dynamics analysis of a novel 5-DOF parallel machine tool［D］. Qinhuangdao:Yanshan University，2005.(in Chinese)