六自由度雙臂機器人動力學分析與運動控制

吳 俊

(1.北方工業大學計算機學院，北京 100144；2.大規模流數據集成與分析技術北京市重點實驗室，北京 100144)

六自由度雙臂機器人動力學分析與運動控制

吳 俊1,2

(1.北方工業大學計算機學院，北京 100144；2.大規模流數據集成與分析技術北京市重點實驗室，北京 100144)

人形機器人運動過程中穩定持物是機器人領域的一個關鍵研究內容，作為人形機器人中的重要研究方向，六自由度雙臂機器人合作抓持重物的穩定性和安全性難以保證。通過對機器人持物運動過程中的受力和運動分析，提出了基于配置構型評估和動力學分析的控制方法，有效解決了持物過程中的穩定性問題。通過對軌跡點的局部最優調整抓取姿態，利用雙臂操作過程中受力分析，借助典型的LuGre摩擦模型，構建摩擦力和滑動接觸數理模型，并進行動力學分析，在此基礎上，提出了相應的控制方法。通過數值仿真，驗證了在此控制方法下六自由度雙臂能夠平穩有效的持物運動。

六自由度；雙臂機器人；抓取質量評估；LuGre摩擦；動力學分析

0 引言

在家用和工業機器人繁榮發展的今天，人類對機器人的需求也更加多樣。其中雙臂機器人包絡抓取在工業、救援、看護、軍事等方面有著越來越廣泛的需求。雙臂抓取是一種利用多關節表面操作一個物體的一種操作，在人形機器人研究領域受到廣泛關注。之前的很多研究人員用具有很多自由度的連桿來實現對物體的操作，但卻很容易由于外界的干擾和故障而失敗。相比而言，用多個機械臂包圍抓取一個物體即包絡抓取，卻能在保證操作穩定性和健壯性的同時抓取更大更重的物體，它能通過把重力分散到各個接觸點從而減小關節所需的驅動力，而且在很多情況下，包絡抓取是唯一的解決辦法。盡管有很多優點，由于“包絡抓取”在操作中接觸點不停在變的這一特性，可能會導致抓取不穩定而不能抵抗外力的影響。

文獻[1][2]中用了多種方式來評估抓取質量，得到每個位置的最佳抓取配置構型，但由于實驗對象較輕而并沒有重點考慮動力學特征，使運動的不可靠性增加。Kazuki Tamura等人對機器臂抓取圓柱形物體受力與運動進行了分析與研究，利用LuGre摩擦模型對雙臂和目標物體之間接觸點的摩擦動力學進行建模[3]，但僅針對平面二自由度。

基于以上問題和研究現狀。為使六自由度雙臂完成抱球情況下的軌跡運動，提出了以抓取質量評估和動力學建模為基礎的控制方法，從全局和局部分別考慮了如何達到預期的效果，在全局的角度，有了路徑之后，分別計算選取了整個路徑上每一點（路徑點密度相關）的最優配置構型。在局部的角度，為了從初始條件到達每一步，需要通過動力學模型的解算來求出關節力矩的表達式，然后通過比例增益的方法以得到合適的力矩，從而驅動每一步。

第二部分講述了如何選取合適的配置構型，第三部分是動力學模型的解算，第四部分根據第二部分和第三部分的內容提出了控制算法并給出數理仿真與實驗結果分析。第五部分總結。

1 配置構型的選取和抓取效果評估

配置構型即抓取矩陣的計算依賴于物體的重心位置、各個接觸點位置、基坐標、各個關節的位移和承重，具體的計算過程參照Domenico Prattichizzo, Jeffrey C.Trinkle在Springer機器人手冊中關于抓取的章節[4]。

在計算出每個點的配置構型之后，需要根據抓取質量的評估方法來選出最終的配置構型。在[1][5]中有抓取質量測量方法的廣義集合。抓取質量用來評估抓取的效果和抓取穩定性，重力存在的情況下，有兩個抓取質量評估方法對于一個穩定的包絡抓取至關重要，在這里把它們分別以二分之一的權重相結合以完成配置構型的選取：

（1）最小慣性區域：這里采用的區域選擇法的評價標準是使彌補重力和慣性力所需要的力能夠最小。典型做法是通過縮小物體重心和抓取中心的距離，其中抓取中心是以接觸點為頂點形成的多面體的體心。理想情況是接觸力的方向和物體重心方向相交從而能抵消重力和慣性力。

最小慣性區域是物體表面的某些部分，這些部分的摩擦錐包含物體重心。在這些區域的接觸點有更強的能力來彌補慣性力。最小慣性區域定義為滿足以下條件的點的集合[1][5]，

O是物體表面所有點的集合，n(p)是內向物體表面的單位法向量，c(p）是從p指向物體重心Cm的單位中心向量，μ是摩擦系數。

（2）重力補償系數

為了減小抓取和操作物體所需的力矩，盡可能的減小物體所受到的內力是很有必要的。在與環境沒有任何碰撞的情況下，物體所受到的最大的外力是重力，若能使接觸點法向量與重力反向，則會非常有利。為了這個目的，定義了以下的抓取質量評估方法：

g是重力的單位向量。理想的情況是當接觸點法向重合于重力時，關節應用很小的力矩就能抵消重力的作用，此時此項系數為零。

（3）最終抓取排名和配置構型選擇：分兩步完成理想接觸點和理想配置構型的選擇：第一步，假設物體一開始放在理想的位置，在物體表面，通過前一個接觸點的相鄰位置找到最小慣性區域。然而，考慮到雙臂機器人自由度較少，有可能沒有可行的配置構型包含這個接觸點。因此，需要通過求解逆運動學，從而在最小慣性區域內找到包含此接觸點的可行的配置構型。值得注意的是，在實時應用場景中，由于以矩陣為運算單位，計算量大，為了減少計算時間，可以根據時間限制減少每一步的候選配置構型數。第二步，所有通過第一步評估的配置構型將進行重力補償系數的評估。最終綜合上述兩個指數定義了全局質量評估指數：

qi和 qi,init分q別是評估指數和初始值，Wi是權重參數。根據兩個質量評估方式的權值確定了最終的全局質量指數（需要正規化），這個指數是最終選擇合適構型的基礎。

2 動力學分析

在選取了每個位置較優的配置構型后，需要合適的力驅動雙臂使整體達到相應的配置構型并完成每一步的運動。這里采用經典的拉格朗日動力學分析方法分析來解決系統的動力學問題。

2.1 六自由度雙臂系統

圖1展示了抱有球形物體的六自由度雙臂系統的實體建模和坐標設置。

圖1 抱球六自由度雙臂系統Fig.1 6-DOF dual-arm with a ball

假設以下條件成立：

（1）機器人兩個手臂都由2個剛性連桿和3個活動關節；

（2）把剛性球作為大而重的物體來考慮；

（3）在兩個手臂上的4個接觸點考慮靜態和動態摩擦力；

（4）機器人關節摩擦力忽略不計；

假定抱定的物體是質量均勻的球體，所以兩個手臂同步一致。

各個記號的定義如下：

l1和l2分別是連桿1、3和2、4的長度,m1，m2和m0分別是連桿1、3，連桿2、4和物體的質量,I1,I2和I0是關節1、3，關節2、4和物體的轉動慣量，r是物體的半徑。

依據機器人在關節空間中的動力學方程封閉形式的一般結構式。它反映了關節力或力矩與關節變量、速度和加速度之間的函數關系。機器人的運動方程和帶有靜態和動態摩擦力的系統的動力學建模如下：

簡要的表示為：

2.2 靜態和動態摩擦力分析

這里采用LuGre摩擦模型，該模型用兩個接觸面間彈性剛毛的平均偏移來表征摩擦力的動態行為。接觸點的靜態和動態摩擦力定義為以下單一的等式[5][6]。產生的摩擦力用以下式子計算：

σ0和σ1分別是剛毛的剛性和彈性，σ2是流體粘性摩擦系數，z是平均剛毛形變，y是機器人手臂的肘與接觸點之間的距離，y˙是機器人相對于物體的滑動速度。下標i（=1，2）表示是linki的變量。速度無關函數。

FC和FS分別是庫侖摩擦力和靜態摩擦力。參數vs是Stribeck速度，決定了g(y˙)以多快的速度接近FC。當y˙（滑動速度）很快時，摩擦力收斂到FC，相反則收斂到FS。

四個接觸點 A, B, C, D選取A, B分析，其余兩點相似，則A,B相對于原點O可以表示為：

2.3 接觸力分析

接下來重點描述滑動接觸的細節，球心也就是

其中Bz是接觸點 B的 z軸坐標，又有 r/y=tan(θ/2)所以y=r/tan(θ/2)代入上式并對其關于時間求導數，能夠得到

3 控制算法與數理仿真

3.1 控制算法

通常，靜態和動態摩擦是不能實時計算的，這一點對于進行數值仿真是很重要的。控制算法如下：

第一步：路徑和抓取姿態選取

1）給出理想的路徑。在目標路徑上選擇 m個離散點作為子目標，子目標之間的連接形成了對數理路徑的模擬逼近，m的大小取決于路徑跟蹤的精度。

2）基于物體在每一點的位置信息，通過求解逆運動學，得到相應點可靠的配置構型。

第二步：動力學模型解算

2）把計算出來的Wμμλ作為外力帶入式（8），并通過式（24）計算接觸力（完整約束力）向量 Sλ 。

3）利用 Sλ 和Wμμλ 計算角加速度矢量

其中τ是控制輸入。

第三步：控制輸入

其中xd是理想軌跡，并且KD∈R2X2, KP∈R2X2兩者分別是導數和比例增益矩陣。

3.2 參數設置與仿真

在路徑的離散點上找出相應的離散點，并找到相應位置最佳的配置構型，得到相應位置的抓取矩陣，假設摩擦系數μ1=μ2=μ3=μ4=0.1，理想路徑是[0.3+0.11cos((2π/T)t)-0.1+0.11 sin((2π/T)t)],系統的其它參數包括l1=l2=0.5 m, m1=m2=10.0 kg ,m0=20.0 kg,其中與 LuGre摩擦模型相關的參數設定為 σ0= 5000 N/m，σ1=632 kg/s,σ2=0.0 kg/s，υs=0.001 m/s, 綜合質量抓取方法中的?1=?2=1/2。

利用matlab完成了仿真實驗，圖2和圖3分別展示了當KD=30I2,KP=225.0I 2和當KP=50I2的情況下抓取物體的重心運動軌跡曲線，可以看出采用這里提出的控制方法在兩種參數設置下走出的實際運動曲線都很接近理想曲線從而證明具有很好的穩定性。

圖2 KP=225.0I2效果圖Fig.2 The effect picture when KP=225.0I2

圖3 KP=50I2效果圖Fig.3 The effect picture when KP=50I2

4 結論

首先分析了針對本系統運動過程中的最佳抓取位姿信息，然后詳細分析了六自由度雙臂操作系統的動力學特征，其中利用了LuGre摩擦模型對復雜的摩擦力進行分析，最后進行了數理仿真驗證了基于抓取f質量和動力學分析控制方法的有效性。

未來將針對控制力矩的輸入做更多的研究，并將動力學分析應用到自由度更靈活的系統上。

[1]Behzad Talaei, Farzaneh Abdollahi, H.A.Talebi.Whole Arm Manipulation Planning Based on Grasp Dynamic Properties and Sampling-Based Algorithms with Kinematic Analysis[C]//20th Iranian Conference on Electrical Engineering (ICEE2012), Tehran, Iran: IEEE, 2012: 963-968.

[2]R.Suarez, M.Roa, and J.Comella.Grasp quality measures[R], Technical Report, Technical University of Catalonia, 2006.

[3]Kazuki Tamura, Fumihiko Asano.Whole Arm Manipulation with Effects of Static and Dynamic Friction on Body Surface[C]//11th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI).Kuala Lumpur, Malaysia: IEEE, 2014: 273-278.

[4]Bruno Siciliano Prof., Oussama Khatib Prof.Springer Handbook of Robotics.Springer Berlin Heidelberg.2008: 671-702.

[5]B.Talaei, H.A.Talebi, F.Abdollahi.Whole arm manipulation planning and control[D].M.Sc.thesis.Tehran, Iran: Department of Electrical Engineering, Amirkabir University of Technology, 2012.

[6]Anderson DO, Moore JB.Optimal filtering[M].Prentice-Hall; 1979.

[7]M.Ding, R.Ikeura, T.Mukai, H.Nakashima, S.Hirano, K.Matsuo, M.Sun, C.Jiang and S.Hosoe.Comfort estimation during lift-up using nursing care robot-RIBA[C]//Proc.of the First Int.Conf.on Innovative Engineering Systems.Alexandria, Egypt: IEEE, 2012:246-250.

[8]D.Prattichizzo and J.C.Trinkle.Springer Handbook of Robotics: Chapter 28, Grasping[M], pp.647-669, 2006.

[9]Anderson DO, Moore JB.Optimal filtering [M].Prentice-Hall; 1979.

[10]D.Hoshino, M.Izutsu, N.Kamamichi and J.Ishikawa.Friction compensation control based on the LuGre model[C].JSME Conference on Robotics and Mechatronics: No.11-5.Okayama, Japan, 2011: 2A2-K08 (1)-2A2-K08 (4).

[11]K.J.Astrom, C.Canudas De Wit.Revisiting the LuGre friction model[J].IEEE Control Systems Magazine, 2008, 28(6): 101-114.

[12]Li Y, Tong S, Li T.Adaptive fuzzy output feedback control for a single-link flexible robot manipulator driven DC motor via backstepping[J].Nonlinear Analysis Real World Applications, 2013, 14(1): 483-494.

[13]Dimaio SP, Salcudean SE.Optimal selection of manipulator impedance for contact tasks[C]//Proceedings of IEEE international conference on robotics and automation, 2004, 5: 4795-4801.

Dynamic Analysis and Grasp Control Method of 6-DOF Dual-arm Robot

WU Jun1,2
(1.College of Computer Science and Technology, North China University of Technology, 100144; 2.Beijing Key Laboratory on Integration and Analysis of Large-scale Stream Data)

Steady grasp of humanoid robot under motion conditions is a key research field.As a new research direction of humanoid robot, the steady and secure grasp of huge and weight object of 6-DOF dual-arm robot is still difficult to guarantee.With the analysis of load and motion in the grasp process, proposed a new method based on configuration evaluation and kinematic analysis, using force analysis and conventional LuGre friction model, built model of friction and sliding friction, using kinematic analysis, based on all above, new control method proposed.It was verified that using the method, the 6-DOF dual-arm’s grasping option can be stable and effective.

6-DOF; Dual-arm robot; Grasp quality evaluation; LuGre friction; Dynamics analysis

TP24

A

10.3969/j.issn.1003-6970.2017.03.026

吳俊(1993)，男，碩士，研究生，主要從事分布式系統，多機器人系統。

本文著錄格式：吳俊.六自由度雙臂機器人動力學分析與運動控制[J].軟件，2017，38（3）：128-132