基于Matlab機(jī)械臂力控系統(tǒng)仿真研究

覃海強(qiáng),熊慶宇,2,石 欣,王 楷

QIN Haiqiang1,XIONG Qingyu1,2,SHI Xin1,WANG Kai1

1.重慶大學(xué) 自動(dòng)化學(xué)院，重慶 400044

2.重慶大學(xué) 軟件學(xué)院，重慶 400044

1.School of Automation，Chongqing University，Chongqing，400044，China

2.School of Software Engineering，Chongqing University，Chongqing，400044，China

1 引言

隨著科技的進(jìn)步，機(jī)械臂在生產(chǎn)自動(dòng)化中占有越來越重要的地位。機(jī)械臂在執(zhí)行裝配、拋光、去毛刺等任務(wù)時(shí)，都會(huì)與環(huán)境相互接觸，并且產(chǎn)生相互的作用力，太大的作用力會(huì)損壞機(jī)械臂及其加工工件, 因此必須對(duì)機(jī)械臂的受力進(jìn)行有效控制。為了有效地實(shí)現(xiàn)復(fù)雜機(jī)械臂系統(tǒng)的力控制并降低風(fēng)險(xiǎn)節(jié)約成本，對(duì)機(jī)械臂力控制系統(tǒng)進(jìn)行仿真驗(yàn)證是一種有效的途徑。SimMechanics是Matlab 中的機(jī)構(gòu)系統(tǒng)模塊集，它可以對(duì)各種運(yùn)動(dòng)副連接的剛體進(jìn)行建模與仿真，達(dá)到對(duì)機(jī)構(gòu)系統(tǒng)進(jìn)行分析與設(shè)計(jì)的目的。SimMechanics與其它仿真平臺(tái)相比，最大的優(yōu)點(diǎn)是無需建立機(jī)構(gòu)運(yùn)動(dòng)的數(shù)學(xué)模型和編制程序，并且具有系統(tǒng)建模方便直觀和仿真功能強(qiáng)大的特點(diǎn)，能大大減輕設(shè)計(jì)人員的工作，為機(jī)械系統(tǒng)的建模仿真提供一個(gè)強(qiáng)大而方便的平臺(tái)。因此，本文采用 SimMechanics仿真平臺(tái)，對(duì)二自由度機(jī)械臂力控系統(tǒng)進(jìn)行仿真與研究，進(jìn)而實(shí)現(xiàn)對(duì)工業(yè)機(jī)械臂在完成特定作業(yè)時(shí)與環(huán)境間接觸力的控制。

力控就是對(duì)機(jī)器人末端執(zhí)行器輸出力或關(guān)節(jié)力矩的控制。最早將力控用于多關(guān)節(jié)機(jī)器人上的是Whitney，他將力傳感器用在多關(guān)節(jié)機(jī)器人上，并用解運(yùn)動(dòng)速度的方法（RMRC）推導(dǎo)出力反饋控制的向量表達(dá)式[1]。P.Fraisse, F.Pierrot和P.Dauchez首先提出虛擬環(huán)境下的機(jī)械臂力控制, 此方法可以增強(qiáng)機(jī)械臂力控制方案的魯棒性[2]；Gabriel Afonso和J.Norberto Pires針對(duì)工業(yè)機(jī)械臂在進(jìn)行拋光作業(yè)的過程中提出一種行之有效的力控方法，使機(jī)械臂能夠以期望的接觸力在期望的位置上運(yùn)動(dòng)，最終完成機(jī)械臂的拋光作業(yè)[3]。

本文根據(jù)機(jī)械臂的拋光任務(wù)，以平面二自由度機(jī)械臂為研究對(duì)象，對(duì)其進(jìn)行動(dòng)力學(xué)分析，并在Matlab/SimMechanics中建立其機(jī)械模型與力控系統(tǒng)，在平面中模擬機(jī)械臂末端與環(huán)境間的接觸面，實(shí)現(xiàn)對(duì)平面二自由度機(jī)械臂的力控，使機(jī)械臂在力控制下完成預(yù)期的運(yùn)動(dòng)軌跡。同時(shí)設(shè)計(jì)振蕩抑制控制器，實(shí)現(xiàn)機(jī)械臂與剛性環(huán)境碰撞接觸過程中沖擊振蕩階段的振蕩抑制。

2 機(jī)械臂動(dòng)力學(xué)模型

不考慮摩擦力等外界干擾的作用，n自由度機(jī)械臂動(dòng)力學(xué)方程為[4]：

其中，M(q)為正定的慣性矩陣；h(q, q˙)為離心力和哥氏力矢量；G(q)為重力矢量；q為關(guān)節(jié)位置；τ為關(guān)節(jié)驅(qū)動(dòng)力矩。

圖1 平面二自由度機(jī)械臂

在平面中建立二自由度機(jī)械臂模型如圖 1所示，假設(shè)機(jī)械臂為剛性，兩個(gè)關(guān)節(jié)均為轉(zhuǎn)動(dòng)關(guān)節(jié)，且每個(gè)連桿的質(zhì)量都集中在連桿的末端。兩個(gè)連桿的質(zhì)量分別為m1和m2；長(zhǎng)度分別為l1和l2；關(guān)節(jié)角位置為；關(guān)節(jié)力矩為

平面二自由度機(jī)械臂動(dòng)力學(xué)方程為：

在笛卡爾坐標(biāo)系中，當(dāng)機(jī)械臂末端與外部環(huán)境相互接觸時(shí)[5]，機(jī)械臂與外界將會(huì)產(chǎn)生相互的作用力Fe，為了保持機(jī)械臂的平衡狀態(tài)，必須對(duì)各個(gè)關(guān)節(jié)施加一定的驅(qū)動(dòng)力矩τ，且有，其中，J為機(jī)械臂的雅可比矩陣，JT為機(jī)械臂雅可比矩陣的轉(zhuǎn)置，且有：

通過 JT可將作用在機(jī)械臂末端的笛卡爾力映射成為等效的關(guān)節(jié)力矩。與環(huán)境相接觸時(shí)，機(jī)械臂的動(dòng)力學(xué)方程為：

3 平面二自由度機(jī)械臂力控

3.1 機(jī)械臂的力控

根據(jù)機(jī)械臂動(dòng)力學(xué)，設(shè)計(jì)機(jī)械臂的力控系統(tǒng)如圖 2所示，其中，F(xiàn)d為笛卡爾坐標(biāo)系中機(jī)械臂末端所受的期望外力；Fe為力傳感器反饋的機(jī)械臂末端力信號(hào)； JT機(jī)械臂雅可比矩陣的轉(zhuǎn)置；Kfp和Kfi分別為比例增益與積分增益；CTH為力變換矩陣。

圖2 機(jī)械臂力控系統(tǒng)結(jié)構(gòu)圖

3.2 平面二自由度機(jī)械臂的力控

在 Matlab/SimMechanics環(huán)境下對(duì)平面二自由度機(jī)械臂系統(tǒng)進(jìn)行力控仿真與研究的思路為：在笛卡爾坐標(biāo)系中，機(jī)械臂末端沿著圓的內(nèi)側(cè)（接觸面）運(yùn)動(dòng)，在運(yùn)動(dòng)過程中，機(jī)械臂末端與圓內(nèi)側(cè)時(shí)刻保持接觸，且相互作用力為一恒定值Fe（忽略機(jī)械臂末端與環(huán)境間的摩擦力）。為了保持機(jī)械臂的在運(yùn)動(dòng)過程中的平衡狀態(tài)，必須對(duì)各個(gè)關(guān)節(jié)施加一定的驅(qū)動(dòng)力矩τ（在此，平面二自由度機(jī)械臂的關(guān)節(jié)數(shù)恰與受控力的維數(shù)相匹配），根據(jù)式(5)可得所需施加的驅(qū)動(dòng)力矩τ為：

其中，F(xiàn)x和Fy是機(jī)械臂末端與外界相互作用的期望力Fe在笛卡爾坐標(biāo)系上的分解，在機(jī)械臂運(yùn)動(dòng)過程中Fx和Fy時(shí)刻變化，合力Fe大小保持不變，方向?yàn)閳A的法向且由圓心指向圓外，如圖 3所示。

整個(gè)力控過程即為控制機(jī)械臂在一定時(shí)間內(nèi)沿著一個(gè)圓的內(nèi)側(cè)運(yùn)動(dòng)，并保持在運(yùn)動(dòng)過程中，機(jī)械臂與圓內(nèi)側(cè)的相互作用力Fe為一恒定值。

圖3 平面二自由度機(jī)械臂力控圖

3.3 接觸力模型

當(dāng)機(jī)械臂末端與環(huán)境接觸時(shí)，根據(jù)胡克定律，環(huán)境施加于機(jī)械臂的作用力Fe可作為彈性恢復(fù)力來模擬：

其中，矢量ΔXE表示由接觸引起的環(huán)境局部變形，由圖 4可知，當(dāng)接觸時(shí) Δ XE=X-XE；當(dāng)不接觸時(shí)，ΔXE=0。KE描述環(huán)境的剛度。式(7)中忽略了摩擦力的作用，環(huán)境剛度KE和變形量ΔXE是方便又理想的表示方法，接觸力Fe作為機(jī)械臂和環(huán)境集中變形來考慮，會(huì)更為準(zhǔn)確。

圖4 機(jī)械臂末端與環(huán)境接觸模型

3.4 沖擊振蕩階段的抑制控制

機(jī)械臂與剛性環(huán)境碰撞接觸的過程可分為三個(gè)階段：接近運(yùn)動(dòng)階段、沖擊振蕩階段、阻尼振蕩階段和穩(wěn)定階段[6]，如圖 5所示。

圖5 機(jī)械臂與剛性環(huán)境的碰撞接觸過程

在機(jī)械臂與環(huán)境處于沖擊振蕩階段時(shí)，兩者之間將會(huì)產(chǎn)生一系列的脈沖力，如果脈沖力過大，必將會(huì)造成機(jī)械臂或環(huán)境的損壞。因此，需設(shè)計(jì)沖擊振蕩抑制控制器以削弱沖擊振蕩階段的振動(dòng)幅值，縮短沖擊振蕩持續(xù)時(shí)間。

沖擊振蕩階段的抑制控制原理如圖6所示，其中，圖 6的（a）表示沖擊振蕩過程中機(jī)械臂末端的位置，虛線部分表示開環(huán)控制時(shí)的位置，實(shí)線部分表示通過抑制控制所期望達(dá)到的效果。圖6的（c）表示向機(jī)械臂關(guān)節(jié)所提供的驅(qū)動(dòng)力，整個(gè)抑制控制的過程即為依據(jù)機(jī)械臂末端位置的不同而改變關(guān)節(jié)驅(qū)動(dòng)力的大小，最終實(shí)現(xiàn)振蕩的抑制。圖6的（b）為機(jī)械臂關(guān)節(jié)角速度-時(shí)間圖，用來確定τmax與τd的切換時(shí)機(jī)，為切換的速度閾值。

圖6 沖擊振蕩階段的抑制控制

圖 7詳細(xì)給出了一個(gè)完整的振蕩周期的抑制控制原理。ta、ts、tb、tc分別表示振蕩周期的開始、驅(qū)動(dòng)力切換、零速和振蕩周期結(jié)束的時(shí)間。

圖7 一個(gè)振蕩周期的抑制控制原理

當(dāng)t∈[ta, ts]時(shí)，對(duì)機(jī)械臂施加能提供的最大關(guān)節(jié)驅(qū)動(dòng)力τmax，τmax=JTFe，F(xiàn)e為反饋的機(jī)械臂末端的接觸力，以此實(shí)現(xiàn)最大化的振蕩抑制，降低后續(xù)振蕩能量；當(dāng)t∈[ts, tc]時(shí)，關(guān)節(jié)力矩則取較小的值τd，τmax=JTFd，F(xiàn)d為期望的機(jī)械臂末端接觸力，以此滿足對(duì)期望接觸力的控制。機(jī)械臂驅(qū)動(dòng)力在τmax與τd之間切換，主要的依據(jù)是切換時(shí)間ts，分析沖擊振蕩過程可知，對(duì)ts的選取，需滿足以下兩個(gè)條件：

條件1：需保證當(dāng)機(jī)械臂末端速度q˙(t)≥0(t∈[tb, tc])時(shí)，驅(qū)動(dòng)力τmax必須停止作用從而減小機(jī)械臂恢復(fù)運(yùn)動(dòng)的速度。

條件2：盡可能長(zhǎng)地增加τmax的作用時(shí)間，從而最大限度地削弱系統(tǒng)的振蕩能量。

ts具體計(jì)算過程如下：

步驟 1：根據(jù)條件1，ts必須選在tb之前。如圖 7所示，定義δt為tb與ts的時(shí)間差，且在tb時(shí)刻機(jī)械臂關(guān)節(jié)角速度q˙為0，則有：

其中，Jm為機(jī)械臂等效慣量。

其中Fe為反饋的接觸力，F(xiàn)e=0表示未接觸狀態(tài)，F(xiàn)e＞0表示接觸狀態(tài)。

4 平面二自由度機(jī)械臂的力控仿真與結(jié)果

4.1 SimMechanics仿真

在 Matlab/SimMechanics中搭建的平面二自由度機(jī)械臂力控系統(tǒng)如圖 8所示，其中包括平面二自由度機(jī)械臂的機(jī)械模型、期望力、力傳感器以及力控制器模塊[8][9][9]。

圖8 基于Matlab/SimMechanics的力控系統(tǒng)

平面二自由度機(jī)械臂的機(jī)械模型如圖 9所示，其中，兩個(gè)Revolute為轉(zhuǎn)動(dòng)副，表示機(jī)械臂的兩個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)；兩個(gè) body表示機(jī)械臂的兩個(gè)連桿，連桿的具體參數(shù)為：長(zhǎng)度 l1=l2=0.35m ，質(zhì)量m1=m2=0.747kg ，相對(duì)于 Z軸的轉(zhuǎn)動(dòng)慣量為IZ1=IZ2=0.0424kg*m2；Joint Sensor為關(guān)節(jié)傳感器，其輸出信號(hào)為關(guān)節(jié)的角度矢量q、角速度矢量以及角加速度矢量˙；Body Sensor輸出信號(hào)為機(jī)械臂末端笛卡爾坐標(biāo)系下的位置坐標(biāo)x、y以及z，由于二自由度的機(jī)械臂只能控制二維方向上的位置或力，因此不對(duì)z軸數(shù)據(jù)做處理。

圖9 基于SimMechanics的二自由度機(jī)械臂機(jī)械模型

根據(jù)式(7)，構(gòu)造機(jī)械臂末端力傳感器模型如圖10所示，取環(huán)境剛度KE=80000N/m；ΔXE=X -XE，XE設(shè)為期望機(jī)械臂末端的運(yùn)動(dòng)軌跡，即為圓的半徑，取R=XE=0.5m；X為笛卡爾坐標(biāo)系下，機(jī)械臂末端在運(yùn)動(dòng)過程中與軌跡圓心的距離（為了簡(jiǎn)單起見，取軌跡圓心與機(jī)械臂第一個(gè)固定轉(zhuǎn)動(dòng)關(guān)節(jié)重合）。由式(7)可知，當(dāng)機(jī)械臂與環(huán)境（即期望的軌跡）沒有接觸時(shí)，將不受到環(huán)境的外力；當(dāng)機(jī)械臂與環(huán)境接觸時(shí)，將產(chǎn)生相互的作用力Fe。這樣就能夠在平面中，模擬出機(jī)械臂與環(huán)境的接觸面。

圖10 基于SimMechanics的力傳感器模型

機(jī)械臂末端期望的受力模型如圖 11所示。在機(jī)械臂與環(huán)境接觸前，給機(jī)械臂末端施加一個(gè)沿著接觸面的法向，大小為8N的力Fe。在這個(gè)力的作用下，機(jī)械臂將向著期望的環(huán)境靠近，并最終與環(huán)境接觸。在接觸后，此期望力Fe即為機(jī)械臂與環(huán)境間相互作用的平衡力。

圖11 基于SimMechanics的機(jī)械臂末端期望力模型

力控制器則是在Matlab中，將M文件嵌入到Simulink 仿真環(huán)境中，依據(jù)式(6)的機(jī)械臂動(dòng)力學(xué)方程，對(duì)系統(tǒng)進(jìn)行力控。同時(shí)，根據(jù)式(8)、(9)對(duì)機(jī)械臂與剛性環(huán)境碰撞接觸過程中的沖擊振蕩階段進(jìn)行抑制控制。

4.2 仿真結(jié)果

具體的仿真結(jié)果如圖 12、圖 13、圖 14、圖 15所示。SimMechanics仿真環(huán)境中的平面二自由度機(jī)械臂的機(jī)械模型如圖 12所示。

圖12 基于SimMechanics的二自由度機(jī)械臂模型

機(jī)械臂末端的運(yùn)動(dòng)軌跡如圖 13所示，通過對(duì)機(jī)械臂末端接觸力的控制，實(shí)現(xiàn)了機(jī)械臂末端運(yùn)動(dòng)軌跡的規(guī)劃。

圖13 機(jī)械臂末端運(yùn)動(dòng)軌跡

當(dāng)未對(duì)碰撞接觸過程中的沖擊振蕩階段進(jìn)行抑制控制時(shí)，機(jī)械臂末端與環(huán)境間的法向接觸力如圖 14所示。可見，機(jī)械臂與環(huán)境相接觸時(shí)，特別是在沖擊振蕩階段，兩者之間產(chǎn)生一系列的脈沖力，相互間的碰撞相當(dāng)劇烈，因此，有必要設(shè)計(jì)沖擊振蕩抑制控制器以削弱沖擊振蕩階段的振動(dòng)幅值，縮短沖擊振蕩持續(xù)時(shí)間。

圖14 未進(jìn)行沖擊振蕩抑制控制的法向接觸力

對(duì)機(jī)械臂與環(huán)境碰撞接觸過程中的沖擊振蕩階段進(jìn)行抑制控制后，機(jī)械臂末端與環(huán)境間的法向接觸力如圖 15所示。實(shí)例仿真結(jié)果表明，沖擊振蕩抑制控制器的使用，有效消弱了沖擊振蕩階段的振動(dòng)幅值、縮短沖擊振蕩持續(xù)時(shí)間。同時(shí)，在機(jī)械臂穩(wěn)定運(yùn)動(dòng)的過程中，其末端與接觸面之間的相互作用力保持為恒定的8N，很好的實(shí)現(xiàn)了特定作業(yè)下，機(jī)械臂與環(huán)境間接觸力的控制。

圖15 進(jìn)行沖擊振蕩抑制控制的法向接觸力

5 結(jié)論

本文主要研究了平面剛性二自由度機(jī)械臂與環(huán)境接觸力的控制問題，在 Matlab/SimMechanics中建立力控系統(tǒng)的仿真模型，解決了機(jī)械臂力控過程中的一些問題：（1）機(jī)械臂力控作業(yè)的環(huán)境描述和運(yùn)動(dòng)建模；（2）在平面中模擬出機(jī)械臂與環(huán)境的接觸面；（3）機(jī)械臂力控作業(yè)的力-運(yùn)動(dòng)軌跡生成；（4）實(shí)現(xiàn)機(jī)械臂末端執(zhí)行器輸出力以及關(guān)節(jié)力矩的控制；（5）實(shí)現(xiàn)機(jī)械臂與剛性環(huán)境碰撞接觸過程中的沖擊振蕩階段的振蕩抑制。此后的工作將要實(shí)現(xiàn)平面剛性二自由度機(jī)械臂力-位混合控制，并將機(jī)械臂由二自由度擴(kuò)展到多自由度，最終實(shí)現(xiàn)多自由度機(jī)械臂的力-位混合控制。

[1]D.E.Whitney.Resolved motion rate control of manipulators and human prostheses[J].IEEE Transactions on Man-Machine Systems, vol.10, pp.47–53, 1969.

[2]P.Fraise, F.Pierrot, P.Dauchez.Virtual environment for robot force control[J].IEEE 1993.219- 224.

[3]G.Afonso, J.N.Pires, and N.Estrela.Force control experiments for industrial applications:A test case using an industrial deburring example[J].Assem.Autom.J., vol.26, no.2, pp.148–156, 2007.

[4]F.L.Lewis, C.T.Abdallah, and D.M.Dawson, Control of Robot Manipulators[M].New York:MacMillan,1993.69-71.

[5]J.T.Wen.Position and Force Control of Robot Arms[J].Proc.IEEE International Symposium on Intelligent Control, Albany, New York, pp.251-256, September 1989.

[6]Nakju Lett Doh,Wan Kyun Chung,Youngil Youm,and Youngwhan Oh.Two Transition Phase Control Methods for Hard Contact[J].ASME Journal of Dynamic System,Measurement and Control, Vol 129, pp.262-274, May 2007.

[7]Yuan Shaoqiang, Liu Zhong, Li Xingshan.Modeling and Simulation of Robot Based on Matlab/SimMechanics[J].Proc.IEEE Symp.Control, IEEE Press,July 2008,pp. 161-165,doi:10.1109/CHICC.2008.4604913.

[8]Wenbin Deng, Jae-Won Lee, Hyuk-Jin Lee.Kinematics simulation and control of a new 2 DOF parallel mechanism based on Matlab/SimMechanics[J].Proc.IEEE Symp. Computing,Communication,Control,and Management(CCCM), IEEE Press,Aug 2010, pp.233-236.doi:10.1109/CCCM.2009.5267894.

[9]薛定宇, 陳陽(yáng)泉.基于MATLAB/ Simulink 的系統(tǒng)仿真技術(shù)與應(yīng)用[M].北京:清華大學(xué)出版社, 2002.