王振斌，李 笑，武交峰

（1.廣州啟帆工業(yè)機(jī)器人有限公司，廣東 廣州 510700；2.廣東工業(yè)大學(xué)機(jī)電工程學(xué)院，廣東 廣州 510006）

1 引言

隨著科技的不斷進(jìn)步，機(jī)器人越來越具備靈活操作性、友好性和開放性等特點(diǎn)。目前，工業(yè)和協(xié)作機(jī)器人通常都利用示教器規(guī)劃運(yùn)動(dòng)軌跡，該方法操作效率低、繁瑣，且要求操作者有一定的機(jī)器人操作基礎(chǔ)。為了快速、便捷和友好的生成機(jī)器人復(fù)雜運(yùn)動(dòng)軌跡，可通過直接示教的方式，即操作者拖動(dòng)機(jī)器人按特定軌跡運(yùn)動(dòng)的示教方式。為實(shí)現(xiàn)良好的拖動(dòng)效果，一般使用力傳感器來實(shí)現(xiàn)，然而力矩傳感器的零點(diǎn)容易漂移且價(jià)格較為昂貴。因此，研究機(jī)器人的無力矩傳感器直接示教方法具有重要的技術(shù)經(jīng)濟(jì)意義。

近年來，國內(nèi)外較多學(xué)者對(duì)機(jī)器人直接示教技術(shù)開展了相關(guān)研究。文獻(xiàn)[1]等在機(jī)器人末端安裝多維力傳感器以實(shí)時(shí)反饋操作者對(duì)末端的牽引信息。在機(jī)器人關(guān)節(jié)電機(jī)處于位置模式下，控制器將末端的受力轉(zhuǎn)化為機(jī)器人關(guān)節(jié)運(yùn)動(dòng)增量，從而使機(jī)器人能夠跟隨操作者的牽引運(yùn)動(dòng)，但是該方案末端感知區(qū)域有限且成本高昂。文獻(xiàn)[2-3]等設(shè)置機(jī)器人關(guān)節(jié)電機(jī)為力矩模式，通過實(shí)時(shí)給機(jī)器人關(guān)節(jié)補(bǔ)償拖動(dòng)其所需克服的重力和摩擦力力矩來實(shí)現(xiàn)直接示教，該方案是一種力開環(huán)示教方式，其靈敏度依賴于動(dòng)力學(xué)模型的準(zhǔn)確程度，而精確的動(dòng)力學(xué)模型很難獲得。文獻(xiàn)[4]等設(shè)置機(jī)器人關(guān)節(jié)電機(jī)為位置模型，通過導(dǎo)納控制將關(guān)節(jié)力矩觀測(cè)器觀測(cè)的外力轉(zhuǎn)化為關(guān)節(jié)的位置增量，從而實(shí)現(xiàn)機(jī)器人的拖動(dòng)，但該方案的靈活性和柔順性欠佳。

針對(duì)以上問題，提出一種估計(jì)外力矩的機(jī)器人直接示教方法。在電機(jī)狀態(tài)處于力矩模型下，通過機(jī)器人的狀態(tài)和動(dòng)力學(xué)模型以及基于廣義動(dòng)量的力矩觀測(cè)器計(jì)算當(dāng)前時(shí)刻機(jī)器人所需補(bǔ)償?shù)闹亓?、摩擦力矩和操作者的牽引外力矩。該直接示教方法具有系統(tǒng)成本低、操作方便靈活和柔順性好等優(yōu)點(diǎn)。

2 直接示教方案

估計(jì)外力矩的機(jī)器人直接示教方法中重力矩和摩擦力矩作為前饋補(bǔ)償，關(guān)節(jié)的估計(jì)外力矩由基于廣義動(dòng)量力矩觀測(cè)器觀測(cè)的外力矩，經(jīng)過導(dǎo)納與PD控制轉(zhuǎn)化所得，作為反饋補(bǔ)償。機(jī)器人連桿重力和關(guān)節(jié)摩擦力模型通過辨識(shí)獲得。

2.1 控制結(jié)構(gòu)

估計(jì)外力的機(jī)器人直接示教原理，如圖1所示。

圖1 估計(jì)外力矩的直接示教原理圖Fig.1 Schematic Diagram of Direct Teaching for Estimating External Torque

拖動(dòng)機(jī)器人運(yùn)動(dòng)時(shí)，實(shí)時(shí)補(bǔ)償機(jī)器人估計(jì)外力矩向量τest、關(guān)節(jié)摩擦力力矩向量τfric和重力矩向量G，其中摩擦力力矩和重力矩向量組成了前饋補(bǔ)償向量τff，估計(jì)的外力矩向量τest是通過PD控制算法得到的反饋補(bǔ)償，具體計(jì)算公式為：

式中：Kp—系統(tǒng)剛度系數(shù)向量；KD—系統(tǒng)的阻尼系數(shù)向量；qref—關(guān)節(jié)期望的位置向量ref—關(guān)節(jié)期望的速度向量；q—關(guān)節(jié)實(shí)際的位置向量—關(guān)節(jié)實(shí)際的速度向量。

關(guān)節(jié)期望的角加速度通過導(dǎo)納控制計(jì)算得到，如式（3）所示。關(guān)節(jié)期望的角速度與位置由角加速度積分所得。

式中：M—慣性矩陣ext—觀測(cè)的關(guān)節(jié)外力力矩向量；D—阻尼對(duì)角矩陣，對(duì)角線元素均為常數(shù)。

2.2 辨識(shí)重力和摩擦力模型

準(zhǔn)確的連桿重力與關(guān)節(jié)摩擦力模型一般通過辨識(shí)相關(guān)的動(dòng)力學(xué)參數(shù)獲得。

2.2.1 機(jī)器人動(dòng)力學(xué)模型

n自由度剛體機(jī)器人動(dòng)力學(xué)模型為：

式中：C(q，)—離心力和哥氏力項(xiàng)矩陣；τ—關(guān)節(jié)力矩向量；τext—外部環(huán)境作用于關(guān)節(jié)的外力矩向量；

關(guān)節(jié)i的摩擦力模型為：

式中：Fci—關(guān)節(jié)i的庫侖摩擦力參數(shù)；Fvi—關(guān)節(jié)i的粘滯摩擦力參數(shù)。

2.2.2 參數(shù)辨識(shí)

動(dòng)力學(xué)參數(shù)由慣性參數(shù)和摩擦力系數(shù)組成［5］，如式（6）所示。

式中：βi—?jiǎng)恿W(xué)參數(shù)向量；XXi、XYi、XZi、YYi、YZi和ZZi—連桿i的慣性張量項(xiàng)；MXi，MYi，MZi—連桿i的一階質(zhì)量矩；Mi—連桿i的質(zhì)量；Iai—關(guān)節(jié)i的電機(jī)慣性矩。

機(jī)器人動(dòng)力學(xué)模型相對(duì)于待辨識(shí)的動(dòng)力學(xué)參數(shù)是線性的。將式（4）線性化表達(dá)為：

式中：?(q，，)—回歸矩陣；β—n個(gè)連桿的動(dòng)力學(xué)參數(shù)向量，其中部分元素對(duì)動(dòng)力學(xué)模型是冗余的，需要與其他元素線性組合為一個(gè)整體進(jìn)行辨識(shí)。

機(jī)器人沿設(shè)計(jì)的激勵(lì)軌跡運(yùn)動(dòng)。采集t1，…，tk時(shí)刻機(jī)器人關(guān)節(jié)的位置、速度、加速度以及力矩值，根據(jù)式（7）得到式（8）：

通過最小二乘法求解式（8）可得動(dòng)力學(xué)參數(shù)向量β的值，形式如下：

2.2.3 模型驗(yàn)證

動(dòng)力學(xué)參數(shù)辨識(shí)的精度由測(cè)量值與計(jì)算值的殘差均方根εRMS來評(píng)定，εRMS值越小辨識(shí)精度越高。

εRMS表達(dá)式，如式（10）所示。

2.3 基于廣義動(dòng)量的力矩觀測(cè)器

基于廣義動(dòng)量的力矩觀測(cè)器，根據(jù)機(jī)器人的狀態(tài)與動(dòng)力學(xué)模型，觀測(cè)機(jī)器人關(guān)節(jié)的牽引外力矩［6］。機(jī)器人的動(dòng)力學(xué)模型滿足：

機(jī)器人的廣義動(dòng)量表達(dá)式為：

將式（4）、式（11）代入式（12）的求導(dǎo)函數(shù)，化簡(jiǎn)得：

式中：r—?dú)埐?；K—比例系數(shù)。

理想條件下，估計(jì)量(q，)(q)和與實(shí)際值一致，結(jié)合式（13）、式（14）對(duì)式（15）進(jìn)行求導(dǎo)得：

將式（16）進(jìn)行拉普拉斯變換，可得下式：

從式（17）可以看出，R是對(duì)Text的跟蹤，穩(wěn)態(tài)時(shí)，R(0)=Text(0)，因此，r可以看作是外力矩的觀測(cè)值，即：

3 驗(yàn)證實(shí)驗(yàn)

使用六軸協(xié)作機(jī)器人進(jìn)行直接示教實(shí)驗(yàn)，機(jī)器人伺服驅(qū)動(dòng)器與控制器之間采用EtherCat通信協(xié)議。實(shí)驗(yàn)平臺(tái)，如圖2所示。主要的硬件型號(hào)，如表1所示。

表1 實(shí)驗(yàn)平臺(tái)主要器件表Tab.1 Main Components of the Experimental Platform

圖2 實(shí)驗(yàn)平臺(tái)Fig.2 Experimental Platform

機(jī)器人的動(dòng)力學(xué)參數(shù)存在相互耦合現(xiàn)象，只能得到一組最小動(dòng)力學(xué)參數(shù)集。辨識(shí)機(jī)器人前三軸動(dòng)力學(xué)最小參數(shù)集結(jié)果，如表2所示。其中較長的動(dòng)力學(xué)參數(shù)用θ表示，具體的形式可參考文獻(xiàn)[7-8]，辨識(shí)用的激勵(lì)軌跡可參考文獻(xiàn)[9-10]。

表2 辨識(shí)參數(shù)結(jié)果Tab.2 Results of Identification Parameters

機(jī)器人跟蹤測(cè)試軌跡時(shí)采集的力矩與通過動(dòng)力學(xué)模型計(jì)算的力矩對(duì)比，如圖3所示。可以看出采集的力矩與通過動(dòng)力學(xué)模型計(jì)算的力矩完全一致。根據(jù)式（10）計(jì)算的殘差均方根，如表3所示。

表3 關(guān)節(jié)力矩殘差均方根Tab.3 Residual Root Mean Square of Joint Torque

圖3 機(jī)器人各軸辨識(shí)效果圖Fig3 Identification Effect Diagram of Each Axis of Robot

可以看出各關(guān)節(jié)力矩殘差均方根較小。綜合圖2和表3可以看出，辨識(shí)得到的動(dòng)力學(xué)參數(shù)有較高的精度，滿足工程使用要求。利用辨識(shí)得到的動(dòng)力學(xué)參數(shù)和機(jī)器人當(dāng)前的運(yùn)動(dòng)信息，可計(jì)算出各項(xiàng)的估計(jì)值，例如(q，)、(q)和等。

為驗(yàn)證所提出方法的有效性，對(duì)估計(jì)外力矩的直接示教方法與未補(bǔ)償外力矩的直接示教方法進(jìn)行了大量的對(duì)比實(shí)驗(yàn)研究。兩種直接示教方法中機(jī)器人關(guān)節(jié)電機(jī)均設(shè)置為力矩模式。估計(jì)外力矩的直接示教方法給關(guān)節(jié)補(bǔ)償估計(jì)外力矩、重力矩和摩擦力力矩。未補(bǔ)償外力矩的直接示教方法給關(guān)節(jié)補(bǔ)償重力矩和摩擦力力矩。實(shí)驗(yàn)時(shí)，機(jī)器人處于靜止?fàn)顟B(tài)，操作者以相同的力從機(jī)器人零位分別拖動(dòng)兩種示教方法下的關(guān)節(jié)旋轉(zhuǎn)一定角度后停止。每次僅拖動(dòng)一個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)，依次對(duì)前三個(gè)關(guān)節(jié)進(jìn)行了實(shí)驗(yàn)研究。機(jī)器人關(guān)節(jié)1、2和3的直接示教實(shí)驗(yàn)，如圖4～圖6所示。關(guān)節(jié)1在估計(jì)外力矩的直接示教實(shí)驗(yàn)中得到的估計(jì)外力矩，如圖4（a）所示。由基于廣義動(dòng)量力矩觀測(cè)器觀測(cè)的外力矩經(jīng)過導(dǎo)納和PD控制轉(zhuǎn)化所得。關(guān)節(jié)1的直接示教實(shí)驗(yàn)速度對(duì)比，如圖4（b）所示。

圖4 關(guān)節(jié)1的直接示教實(shí)驗(yàn)Fig.4 Direct Teaching Experiment of Joint 1

圖中引入估計(jì)力矩速度是從估計(jì)外力矩的直接示教實(shí)驗(yàn)中采集的速度，未補(bǔ)償估計(jì)力矩速度是從無外力矩補(bǔ)償?shù)闹苯邮窘虒?shí)驗(yàn)中采集的速度。從圖4（a）可以看出，在估計(jì)外力矩的直接示教實(shí)驗(yàn)中，拖動(dòng)機(jī)器人關(guān)節(jié)起動(dòng)階段，估計(jì)到操作者施加在機(jī)器人關(guān)節(jié)的起動(dòng)外力矩。拖動(dòng)機(jī)器人關(guān)節(jié)停止階段，估計(jì)到操作者阻止機(jī)器人關(guān)節(jié)運(yùn)動(dòng)的外力矩。

從圖4（b）可以看出，在估計(jì)外力矩的直接示教實(shí)驗(yàn)中，速度在起動(dòng)和停止階段的斜率明顯大于無外力矩補(bǔ)償直接示教實(shí)驗(yàn)中的，說明估計(jì)外力矩的直接示教功能具有起動(dòng)和停止速度快、用時(shí)少等優(yōu)點(diǎn)，圖5、圖6與圖4同理。實(shí)驗(yàn)表明，基義廣義動(dòng)量力矩觀測(cè)器估計(jì)外力矩的直接示教方法比無外力補(bǔ)償?shù)闹苯邮窘谭椒ǜ痈咝?、靈敏和柔順。

圖5 關(guān)節(jié)2的直接示教實(shí)驗(yàn)Fig.5 Direct Teaching Experiment of Joint 2

圖6 關(guān)節(jié)3的直接示教實(shí)驗(yàn)Fig.6 Direct Teaching Experiment of Joint 3

4 結(jié)論

為解決現(xiàn)有直接示教靈活性、柔順性低和成本高等問題，提出了一種基于廣義動(dòng)量力矩觀測(cè)器估計(jì)外力矩的力矩閉環(huán)直接示教方法。實(shí)驗(yàn)表明，估計(jì)外力矩的無力矩傳感器直接示教方法有效，可為無力傳感器機(jī)器人直接示教系統(tǒng)設(shè)計(jì)提供指導(dǎo)。