基于粒子群算法的阻抗控制在機(jī)械臂柔順控制中的應(yīng)用*

周曉東，任天助，張激揚(yáng)，周 銳

(1.北京控制工程研究所 精密轉(zhuǎn)動和傳動機(jī)構(gòu)長壽命技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京100190; 2.北京航空航天大學(xué)自動化科學(xué)與電氣工程學(xué)院，北京100191)

基于粒子群算法的阻抗控制在機(jī)械臂柔順控制中的應(yīng)用*

周曉東1，任天助2，張激揚(yáng)1，周 銳2

(1.北京控制工程研究所 精密轉(zhuǎn)動和傳動機(jī)構(gòu)長壽命技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京100190; 2.北京航空航天大學(xué)自動化科學(xué)與電氣工程學(xué)院，北京100191)

阻抗控制策略是實(shí)現(xiàn)機(jī)械臂的末端力柔順控制的一種重要方法.然而，針對阻抗控制參數(shù)的確定，目前尚缺乏通用算法.粒子群算法具有概念簡單、易行、魯棒性好等特點(diǎn)，適用于阻抗控制參數(shù)的確定與優(yōu)化.通過分析基于力反饋的笛卡爾空間阻抗控制結(jié)構(gòu)，采用粒子群算法整定阻抗控制參數(shù).結(jié)合阻抗控制自身特點(diǎn)對粒子群算法做了相應(yīng)改進(jìn)，并通過仿真驗(yàn)證了優(yōu)化后的阻抗控制算法可實(shí)現(xiàn)對七自由度機(jī)械臂的柔順控制且具有較好的魯棒性.

粒子群算法;柔順控制;阻抗控制;機(jī)械臂

0 引言

目前，應(yīng)用于工業(yè)現(xiàn)場的機(jī)械臂只能完成一些固定軌跡的動作，如焊接、噴涂、搬運(yùn)、碼垛、普通裝配及上下料等動作.這些工種對于機(jī)器人而言都屬于結(jié)構(gòu)化環(huán)境，被操作對象形狀、位置及環(huán)境中的障礙物等信息是預(yù)知的，即實(shí)現(xiàn)預(yù)定軌跡的跟蹤，屬于單純的位置控制.然而，對于某些精密裝配、微操作、人機(jī)協(xié)調(diào)工作、空間機(jī)械臂對非合作目標(biāo)的操控及移動式的服務(wù)機(jī)器人操作等工種，不僅要求機(jī)器人末端執(zhí)行機(jī)構(gòu)能到達(dá)預(yù)定位置，同時對操控力的柔順性、安全性和平滑性要求日益提高.而傳統(tǒng)的基于位置控制的工業(yè)機(jī)器人通常難以實(shí)現(xiàn)兩個裝配件的穩(wěn)定接觸.模型的不確定性和力反饋的缺乏可能導(dǎo)致較大的反作用力，甚至導(dǎo)致零件或機(jī)器人損壞.

基于此，近年來眾多學(xué)者開展了關(guān)于機(jī)械臂力的柔順控制研究［1-4］.通過在機(jī)械臂的腕部、關(guān)節(jié)內(nèi)部或手指等部位引入額外的力/力矩傳感器實(shí)現(xiàn)力感知，再借助相應(yīng)的控制策略實(shí)現(xiàn)對操控力的柔順控制.歸納起來，針對機(jī)械臂的力控制，可分為兩種:“力－位置”混合控制法和阻抗控制.其中，阻抗控制是將未知的環(huán)境等效為一個阻抗模型，將力控制轉(zhuǎn)換為對位置控制的問題［5］.該方法相對力－位置混合控制結(jié)構(gòu)簡單，但難于辨識阻抗控制器的阻抗參數(shù).文獻(xiàn)［6］針對雙臂機(jī)器人的阻抗控制研究提出了一種自適應(yīng)物體阻抗的方法并進(jìn)行了穩(wěn)定性證明，但未具體給出調(diào)節(jié)規(guī)律.文獻(xiàn)［7］介紹了用阻抗控制的方法進(jìn)行機(jī)械臂交互力控制，但其阻抗控制的參數(shù)是根據(jù)經(jīng)驗(yàn)確定的.文獻(xiàn)［8］采用自適應(yīng)魯棒控制策略，并對其阻抗參數(shù)進(jìn)行尋優(yōu).其方法相對比較復(fù)雜，對于14個參數(shù)進(jìn)行尋優(yōu)的過程也并不經(jīng)濟(jì).

本文采用粒子群算法進(jìn)行尋優(yōu)操作，并結(jié)合阻抗控制的特點(diǎn)對粒子群算法進(jìn)行了一定程度的改進(jìn)，使之更加適用于阻抗參數(shù)優(yōu)化問題的求解.并以課題組的七自由度機(jī)械臂進(jìn)行“銷－孔”裝配過程為例，采用仿真的方法，驗(yàn)證了該算法的有效性.

1 阻抗控制的原理與控制結(jié)構(gòu)

1.1 阻抗控制的原理

1985年Hogan［5］率先提出阻抗控制的概念，其核心思想是將幾乎所有的物理系統(tǒng)轉(zhuǎn)化為如圖1所示的“質(zhì)量 －彈簧 －阻尼”系統(tǒng)，其受力情況可表示為

式中:ΔX=Xd－X，Xd為期望位置，X為實(shí)際位置.Kd為期望的剛度矩陣，Bd為期望的阻尼矩陣，Md為期望的慣量矩陣.由此可見，阻抗控制本身是一種將力控制轉(zhuǎn)化為位置控制的方法.

圖1 “質(zhì)量-彈簧-阻尼”系統(tǒng)Fig.1 The“mass-spring-damper”system

阻抗控制從結(jié)構(gòu)上可以分為基于位置的阻抗控制與基于力的阻抗控制［9］，其主要區(qū)別在于后者是以位置信息作為反饋、力控制作為內(nèi)環(huán)，前者是以末端受力信息作為反饋，位置控制作為內(nèi)環(huán).基于位置的阻抗控制策略在工程中獲得了廣泛的應(yīng)用，本文將采用此方法進(jìn)行研究.

1.2 基于力反饋的笛卡爾空間阻抗控制原理

圖2為基于力反饋的笛卡爾空間阻抗控制原理［8］:將接觸力誤差 ΔF=Fd－Fe輸入到阻抗控制器，得到笛卡爾空間的位置修正量 ΔX，將其用于對Xd進(jìn)行修正，即可計(jì)算出位置控制量 Xc，進(jìn)而得到關(guān)節(jié)轉(zhuǎn)角控制量 θc，從而對機(jī)械臂實(shí)施位置控制.

由圖3所示的機(jī)械臂腕部受力與銷子末端接觸力的示意圖可知:圖2中的末端接觸力轉(zhuǎn)化矩陣Te如式(2).其中L1、L2的物理意義如圖3所示.

圖2 基于力反饋的笛卡爾空間阻抗控制原理圖Fig.2 The structure of a impedance control in Cartesian space based on force feedback

圖3 銷子末端接觸力與腕部受力示意圖Fig.3 The illustration of the pin’s contact force andthe manipulator’s wrist force

為了將式(1)應(yīng)用于7-DOF機(jī)械臂的力阻抗控制系統(tǒng)，對其進(jìn)行拉普拉斯變換可得

對式(3)阻抗參數(shù)Md、Bd、Kd進(jìn)行整定優(yōu)化，即可通過圖2所示的控制原理，實(shí)現(xiàn)對機(jī)械臂的柔順控制.

2 阻抗控制參數(shù)的優(yōu)化模型

本文研究的對象為一臺七自由度的冗余機(jī)械臂，其具體作業(yè)任務(wù)為機(jī)械臂抓持一個銷子進(jìn)行插孔操作，通過柔順控制策略實(shí)現(xiàn)對銷子末端與孔底部接觸力的控制.

2.1 機(jī)械臂力反饋模型

為便于研究，本文做如下兩點(diǎn)假設(shè):

(1)銷子與銷孔間的接觸力為單方向的，垂直于孔的底部平面，兩者間不存在相對滑動或滑動趨勢;(2)將銷孔簡化為彈簧模型，設(shè)其等效彈簧剛度系數(shù)為ke.

由假設(shè)(1)可知，該阻抗控制問題轉(zhuǎn)化為一維的，此時式(1)可簡化為式中，kd、bd、md為期望的阻抗參數(shù)，fe和 fd分別實(shí)際接觸力及期望的接觸力，δx為位置誤差

式中，xd為銷子末端的期望位置，x為其實(shí)際位置.則接觸力方程實(shí)施柔順控制策略時，式(6)中的等效彈簧剛度ke通過試驗(yàn)方法獲得.具體方法為:設(shè)銷子末端在x0處時剛好接觸銷孔底部平面，x1處時的接觸力為f1，則

當(dāng)接觸力滿足fe=fd時，即可實(shí)現(xiàn)對銷子與孔接觸力的柔順控制.

2.2 阻抗控制器的設(shè)計(jì)

圖4為本文所提出的阻抗控制器原理圖，該控制器以ΔF(此處為一維，下同)作為輸入，輸出為位置修正量ΔX.

圖4 阻抗控制器原理圖Fig.4 Impedance controller diagram

3 基于粒子群的參數(shù)優(yōu)化方法

3.1 基本粒子群算法設(shè)計(jì)

在尋優(yōu)過程中，粒子群算法需根據(jù)目標(biāo)函數(shù)確定各粒子的適應(yīng)度，適應(yīng)度函數(shù)的優(yōu)劣決定尋優(yōu)質(zhì)量.一般而言，目標(biāo)函數(shù)采用二次型.選取控制誤差與輸出能量綜合起來最小的方向進(jìn)行尋優(yōu)

式中:e為控制誤差、u表示控制器輸出，分別對應(yīng)2.2小節(jié)中的ΔF和ΔX;ρ1為常數(shù)，取值范圍為0～1，根據(jù)需要設(shè)定.參數(shù)優(yōu)化步驟如下:

(1)粒子群初始化

針對被優(yōu)化的阻抗參數(shù)的整定問題，隨機(jī)生成一群初始粒子，其數(shù)值分別對應(yīng)阻抗參數(shù)中M、B、K，由于其大小根據(jù)仿真過程中設(shè)置的彈性模量決定，因此初始粒子的位置在一定范圍內(nèi)等概率分布.并設(shè)置最大迭代次數(shù)以及目標(biāo)函數(shù)的最小值.

(2)更新每個粒子的速度與位置

按照式(9)和式(10)更新粒子的速度與位置

式中:w為權(quán)重因子;i=1，2，…，N為粒子種群中的各個粒子，k=1，2，…，kmax為粒子所處的代數(shù)，則 vki代表第i個粒子處于第k代時的速度，xki代表第i個粒子處于第k代時的位置;xpi為第 i個粒子至今為止的最好位置;xgi則代表至今為止整個種群的最好位置;c1和c2為加速常數(shù)，分別表征當(dāng)代個體與代際群體的學(xué)習(xí)能力大小;nrand1與 nrand2是［0，1］的隨機(jī)數(shù).由式(9)、(10)可知:進(jìn)化粒子的狀態(tài)通過先前經(jīng)驗(yàn)與隨機(jī)過程進(jìn)行更新.

(3)計(jì)算適應(yīng)度

根據(jù)前文提到的目標(biāo)函數(shù)衡量每個粒子的適應(yīng)度.在阻抗控制問題中，目的是使得末端受力接近期望受力，故要使目標(biāo)函數(shù)最小化.

(4)通過比較得出個體最優(yōu)值與群體最優(yōu)值

將每個粒子當(dāng)代的適應(yīng)度與該粒子之前的值作比較.若當(dāng)代的粒子更具適應(yīng)性，即在阻抗控制中適應(yīng)度函數(shù)值更小，則將前代粒子進(jìn)行替換.每個個體進(jìn)行比較之后，選出這一代粒子中最適應(yīng)環(huán)境的個體，作為這一代的全局適應(yīng)度.若這一代的全局的適應(yīng)度優(yōu)于上一代，則更新當(dāng)代全局適應(yīng)度.

(5)算法更新循環(huán)

若不滿足終止條件，重復(fù)步驟(2);當(dāng)滿足終止條件時，退出算法，得到最優(yōu)解.終止條件為達(dá)到最優(yōu)的目標(biāo)函數(shù)值;如果始終無法達(dá)到，則在達(dá)到最大迭代次數(shù)時退出.

3.2 針對阻抗控制問題的改進(jìn)

(1)適應(yīng)度函數(shù)優(yōu)化

對于阻抗控制，要保證系統(tǒng)達(dá)到穩(wěn)態(tài)時末端受力滿足期望值外，還須考慮系統(tǒng)達(dá)到穩(wěn)態(tài)的過渡過程中，力的調(diào)整不宜過大.若在過渡過程中超過一定范圍，勢必會損壞機(jī)械結(jié)構(gòu)，因此須對此進(jìn)行限制，將力反饋的峰值作為適應(yīng)度函數(shù)中的一部分，式(8)轉(zhuǎn)化為

式(11)中Femax代表力反饋的峰值，ρ2為權(quán)值系數(shù)，在仿真實(shí)驗(yàn)中對其進(jìn)行調(diào)整，使其起到作用，又不至于影響過大導(dǎo)致誤差不能正常收斂到0.

(2)進(jìn)行變異操作，防止算法過早陷入早熟

由式(9)可知:粒子下一刻的位置由當(dāng)前位置與當(dāng)前速度共同決定，其中粒子移動的速度跟每一代個體的最優(yōu)位置xpi和整個種群的最優(yōu)位置 xgi有關(guān).然而，當(dāng)粒子當(dāng)前位置很接近一個局部最優(yōu)值時，會表現(xiàn)出很強(qiáng)的趨同性［10］，快速地收斂到一個歷史最優(yōu)值，陷入了“局部最優(yōu)”.基于此，可采用變異的方法增強(qiáng)算法的全局搜索能力，克服收斂到局部最優(yōu)的缺點(diǎn).其變異機(jī)制如下:在每一次迭代中，滿足式(12)變異條件的粒子將產(chǎn)生變異

式中:x(k)為第 k代迭代粒子適應(yīng)度;x(bk)為至第k代粒子群最適應(yīng)值;η為變異系數(shù)，用于判斷粒子與最優(yōu)值接近到何種程度時進(jìn)行變異.

文獻(xiàn)［10］采用多種變異公式根據(jù)概率選擇變異機(jī)制，這種方法可顯著地增加粒子的多樣性，但設(shè)置不同變異公式的概率需要反復(fù)調(diào)節(jié)概率參數(shù)才能發(fā)揮最大優(yōu)勢.這與本文簡化阻抗控制中參數(shù)調(diào)節(jié)的初衷相違背，因此采用簡單隨機(jī)變異

式中n代表0～1之間的隨機(jī)數(shù)，x(k+1)、x(k+1)分別

randminmax為其可以選擇的最大值與最小值，按照3.1小節(jié)中的步驟，將該步置于步驟(5)之前.

3.3 仿真實(shí)驗(yàn)

(1)七自由度機(jī)械臂插銷過程建模

為了對整個阻抗控制系統(tǒng)進(jìn)行驗(yàn)證，首先需建立機(jī)械臂“銷－孔”裝配模型(圖5所示).由于獲得機(jī)械臂的動力學(xué)模型不僅需要進(jìn)行復(fù)雜的運(yùn)動學(xué)正、逆解，且需對接觸面進(jìn)行力學(xué)建模.本文采用多體動力學(xué)仿真軟件ADAMS和MATLAB進(jìn)行聯(lián)合仿真［11］.將模型導(dǎo)入ADAMS并建立機(jī)械臂末端與環(huán)境間的接觸.

圖5 七自由度機(jī)械臂“銷-孔”裝配三維模型Fig.5 A 3D model of the 7-DOF manipulator with“peg-in-hole”assembly

設(shè)機(jī)械臂末端握持的銷子與孔為的材料的彈性模量為E=120 GPa，泊松比為ν=0.25.初始位置時，使銷子末端與孔底平面剛好接觸，接觸部位無變形;然后使機(jī)械臂沿孔的軸線方向移動微小位移 δs，此時 ADAMS測得的接觸力為 fc，由2.1小節(jié)可知

經(jīng)仿真可知:ke=1.787 5×104N/mm.

(2)含有力反饋的閉環(huán)系統(tǒng)仿真過程

系統(tǒng)由一個開環(huán)結(jié)構(gòu)和一個閉環(huán)結(jié)構(gòu)組成.開環(huán)部分的作用是通過軌跡規(guī)劃將銷子從初始位置移動到孔口上方.銷子到達(dá)指定位置后，插孔過程開始，此時啟用力反饋，將ADAMS中計(jì)算的接觸力值作為反饋量輸入到阻抗模塊.經(jīng)計(jì)算得到應(yīng)移動到的位置，再將該位置轉(zhuǎn)化為關(guān)節(jié)坐標(biāo)使機(jī)械臂運(yùn)動.

(3)整定阻抗控制參數(shù)的仿真結(jié)果

設(shè)置阻抗控制的參數(shù) M、B、K(單位分別為: (N·s2)/mm、(N·s)/mm和 N/mm)的初始范圍為［0，100］，粒子群規(guī)模為100，最大迭代次數(shù)為20，參數(shù)整定的結(jié)果如圖6～8所示.

圖6 最優(yōu)個體收斂過程Fig.6 The process of optimal individual convergence

圖7 阻抗參數(shù)收斂過程Fig.7 The convergence process of impedance parameter

圖8 接觸力變化曲線Fig.8 The variation of contact force

為便于驗(yàn)證算法，設(shè)插銷過程中銷子始終保持豎直，直到與孔底面接觸，不受孔壁摩擦力的影響，設(shè)當(dāng)銷子與孔底面的接觸力為200 N時，插銷過程結(jié)束.由圖7可知:這種改進(jìn)的粒子群算法可以使參數(shù)收斂到適宜的值，最終得到的力反饋值平穩(wěn)的達(dá)到期望值，未出現(xiàn)過大超調(diào).

為進(jìn)一步驗(yàn)證算法的魯棒性，考慮將機(jī)械臂的位置控制內(nèi)環(huán)，等效為一個理想的一階慣性環(huán)節(jié).實(shí)際操作是在原有的被控對象模型中串聯(lián)一個一階慣性環(huán)節(jié)，而不改變算法的其他設(shè)置.此時參數(shù)整定的結(jié)果如圖9～11所示.

圖9 考慮內(nèi)環(huán)影響后最優(yōu)個體收斂過程Fig.9 The process of optimal individual convergence with inner-loop

圖10 考慮內(nèi)環(huán)影響后阻抗參數(shù)收斂過程Fig.10 The process of impedance parameter convergence with inner-loop

圖11 考慮內(nèi)環(huán)影響的接觸力變化曲線Fig.11 The variation of contact force with inner-loop

由此可見，即使在系統(tǒng)中引入一階慣性環(huán)節(jié)，改進(jìn)的粒子群算法依然可以使系統(tǒng)按照期望達(dá)到收斂.但值得說明的是其控制效果較之前有了比較大的超調(diào)，其原因在于該算法需要根據(jù)控制系統(tǒng)的不同及時調(diào)整粒子群算法中的有關(guān)參數(shù)，其魯棒性與自適應(yīng)性有待進(jìn)一步提高.

4 結(jié)論

本文將阻抗控制方法與粒子群算法相結(jié)合，提出了基于粒子群算法優(yōu)化的阻抗控制策略并將其應(yīng)用于七自由度機(jī)械臂的力柔順控制.針對基本粒子群算法的一些缺陷，進(jìn)行了適應(yīng)度函數(shù)的改進(jìn)，并引入變異操作.仿真結(jié)果表明:基于粒子群算法的機(jī)械臂阻抗控制參數(shù)優(yōu)化方法簡化了繁瑣的調(diào)參過程，控制效果良好且具有一定的魯棒性.

本文的不足之處在于該方法不能在線確定阻抗參數(shù)，而是需要多次離線的仿真計(jì)算確定，會給工程應(yīng)用帶來不便.且只針對一維的阻抗參數(shù)做了整定和優(yōu)化，后續(xù)研究將考慮實(shí)際的六維力情況，對多維阻抗參數(shù)進(jìn)行在線整定并開展相關(guān)試驗(yàn)研究.

［1］FANNY F，LUIGI V，BRUNO S.Variable impedance control of redundant manipulators for intuitive human-robot physical interaction［J］.IEEE Transactions on Robotics，2015，31(4):850-863.

［2］ETIENNE B，GOWRISHANKAR G，CHENGUANG Y，et al.Interaction force，impedance and trajectory adaptation:by humans，for robots［J］.Springer Tracts in Advanced Robotics，2014，79(1):331-345.

［3］PAREDES M L，GONZALEZ de S P.Dataglove-based interface for impedance control of manipulators in cooperative human-robot environments［J］.Measurement Science and Technology.2013，24(2):1-13.

［4］董曉星.空間機(jī)械臂力柔順控制方法研究［D］.哈爾濱:哈爾濱工業(yè)大學(xué)，2013.DONG X X.Research of compliance control strategies for space robotic arm［D］.Harbin:Harbin Institute of Technology，2013.

［5］HOGAN N.Impedance control:an approach to manipulation［J］.Journal of Dynamic Systems Measurement＆Control，1985，107(1):1-24.

［6］周揚(yáng).雙臂機(jī)器人的控制系統(tǒng)建立及阻抗控制研究［D］.哈爾濱:哈爾濱工業(yè)大學(xué).2014.ZHOU Y.Establishment of control system and research on impedance control for dual-arm robots［D］.Harbin: Harbin Institute of Technology，2014.

［7］GEA J D，KIRCHNER F.Modelling and simulation of robot arm interaction forces using impedance control［J］.World Congress，2008，17(1):15589-15594.

［8］JIANG Z H，KOHNO M.Robotic manipulation of flexible objects based on vibration control using force sensors［J］.Systems，Man and Cybernetics，2002，4(5):363.

［9］ZENG G W，HEMAMI A.An overview of robot force control［J］.Robotica.1997，15(5):473-482.

［10］王苗苗，高岳林.一種新的帶有動態(tài)自適應(yīng)慣性權(quán)重和混合變異的粒子群優(yōu)化算法［J］.計(jì)算機(jī)應(yīng)用與軟件，2010，27(6):70-72.WANG M M，GAO Y L.A new particle swarm optimization with dynamically adaptive inertia weight and hybrid mutation［J］.Computer Applications and Software.2010，27(6):70-72.

［11］PAWLETTA T，F(xiàn)REYMANN B，DEATCU C，et al.Robotic control＆ visualization toolbox for MATLAB［J］.IFAC-PapersOnLine，2015，48(1):687-688.

Application of Impedance Control for a Robotic Manipulator Based on Particle Swarm Optimization Algorithm

ZHOU Xiaodong1，REN Tianzhu2，ZHANG Jiyang1，ZHOU Rui2
(1.Beijing Key Laboratory of Long Life Technology for Precise Rotating and Transmission Mechanism，Beijing Institute of Control Engineering，Beijing 100190，China; 2.School of Automation Science and Electrical Engineering，Beijing University of Aeronautics and Astronautics，Beijing 100191，China)

Impedance control strategy is one of the most important methods for the force and compliance control of robotic manipulators.However，it is difficult to obtain impedance parameters due to the absence of versatile algorithms.According to the structure of Cartesian impedance control based on force feedback，Particle swarm optimization(PSO)algorithm is employed to determine and optimize the impedance parameters via taking advantage of its concision，feasibility and robustness.In addition，the PSO algorithm is improved to some extent in the characteristics of impedance control.Finally，the robustness performance of the optimized impedance control strategy is verified via simulation of compliance control of a 7 DOF(degree-of-freedom)robotic arm.

PSO;compliance control;impedance control;robotic manipulator

TP242.2

A

1674-1579(2016)03-0015-06

10.3969/j.issn.1674-1579.2016.03.003

周曉東(1983—)，男，工程師，研究方向?yàn)橹悄軝C(jī)器人技術(shù);任天助(1992—)，男，碩士研究生，研究方向?yàn)闄C(jī)器人智能控制算法設(shè)計(jì);張激揚(yáng)(1970—)，男，研究員，研究方向?yàn)楹教炱髯藨B(tài)測量和姿態(tài)控制執(zhí)行機(jī)構(gòu);周 銳(1968—)，男，教授，研究方向?yàn)轱w行器制導(dǎo)與控制.

技專項(xiàng)資助項(xiàng)目(Z151100001615067).

2016-01-18