汪 洋，俞建峰，俞俊楠，蔣 毅，化春鍵，錢陳豪

(1.江南大學(xué)機(jī)械工程學(xué)院，江蘇 無錫 214122；2.江蘇省食品先進(jìn)制造裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214122)

0 引言

隨著國內(nèi)建筑數(shù)量的不斷增加，對電梯的使用需求量也逐年遞增。電梯導(dǎo)軌的校準(zhǔn)精度直接影響乘梯舒適性和安全性[1]。在機(jī)器人實(shí)際校準(zhǔn)導(dǎo)軌的過程中，既需要考慮校準(zhǔn)的精度，又需要控制校準(zhǔn)力，以防止因校準(zhǔn)力太大，固定導(dǎo)軌的支架出現(xiàn)斷裂等安全問題。因此，需要研究一種機(jī)器人的位置/力控制策略，有效控制導(dǎo)軌的位置精度和校準(zhǔn)力的大小。

目前，在機(jī)器人的位置/力控制策略[2]中，優(yōu)先保證位置精度，同時控制機(jī)器人接觸力的算法鮮有。因此，本文設(shè)計了一種基于改進(jìn)的粒子群-模糊PD的力位控制系統(tǒng)，并應(yīng)用在電梯導(dǎo)軌校準(zhǔn)機(jī)器人中，期望力可以根據(jù)位置控制要求實(shí)時調(diào)整，并且可根據(jù)機(jī)器人當(dāng)前運(yùn)動狀態(tài)靈活切換控制模式。研究結(jié)果表明，本文所提出的控制策略可以同時保證力和位置的跟隨精度，并且調(diào)整精度滿足國家標(biāo)準(zhǔn)對電梯導(dǎo)軌垂直度偏差范圍的規(guī)定。

1 控制系統(tǒng)方案

當(dāng)電梯導(dǎo)軌校準(zhǔn)機(jī)器人處于不同的工作空間時，要對末端執(zhí)行器的控制需求進(jìn)行相應(yīng)調(diào)整。當(dāng)機(jī)器人在無約束空間運(yùn)動時，應(yīng)著重調(diào)整末端執(zhí)行器的位姿，讓其按照規(guī)劃好的參考軌跡進(jìn)行運(yùn)動，以便快速、準(zhǔn)確地到達(dá)目標(biāo)導(dǎo)軌位置；當(dāng)機(jī)器人末端執(zhí)行器夾持住導(dǎo)軌，進(jìn)行導(dǎo)軌的校準(zhǔn)時，需要控制校準(zhǔn)精度的同時控制導(dǎo)軌的校準(zhǔn)力，使校準(zhǔn)力滿足期望的接觸力，以便使末端執(zhí)行器與導(dǎo)軌之間保持一定的柔順性，在穩(wěn)定可控的期望力下，快速、靈敏地實(shí)現(xiàn)校準(zhǔn)目的。本文控制系統(tǒng)如圖1所示。

圖1 電梯導(dǎo)軌校準(zhǔn)機(jī)器人力位控制系統(tǒng)

圖1中，基于改進(jìn)的粒子群算法和模糊PD(improved particle swarm optimization-fuzzy PD，PsoFuzzyPD)的電梯導(dǎo)軌校準(zhǔn)機(jī)器人力位控制方案由位置內(nèi)環(huán)和阻抗外環(huán)組成。位置內(nèi)環(huán)采用粒子群優(yōu)化模糊PD+前饋動態(tài)控制，模糊控制器的輸入為機(jī)器人關(guān)節(jié)轉(zhuǎn)角誤差和機(jī)器人關(guān)節(jié)角速度誤差，先通過改進(jìn)粒子群算法尋找PD控制器的kP和kD參數(shù)的初值，之后通過模糊自整定技術(shù)自調(diào)節(jié)PD控制器的kP和kD參數(shù)的值，以保證機(jī)器人位置控制的精度。

阻抗控制外環(huán)通過建立的機(jī)器人環(huán)境動力學(xué)模型，先將機(jī)器人末端實(shí)際位移x轉(zhuǎn)換為末端實(shí)際接觸力Fext，之后將Fext與機(jī)器人的末端期望接觸力Fr的差值Fe輸入至阻抗環(huán)節(jié)，輸出調(diào)整量e修正機(jī)器人的期望運(yùn)動軌跡，穩(wěn)定控制電梯導(dǎo)軌校準(zhǔn)機(jī)器人的末端實(shí)際接觸力，從而確保電梯導(dǎo)軌調(diào)整時所受的校準(zhǔn)力可控，保證機(jī)器人能高效、精準(zhǔn)、可控并且安全地調(diào)整電梯導(dǎo)軌。同時，末端期望接觸力根據(jù)實(shí)際調(diào)整需求進(jìn)行限制，并設(shè)計PID調(diào)節(jié)器，通過參考軌跡xr與末端實(shí)際位移x的偏差進(jìn)行末端期望接觸力穩(wěn)定的控制。對電梯導(dǎo)軌校準(zhǔn)機(jī)器人的逆向運(yùn)動學(xué)分析，將笛卡爾空間的參考軌跡xr經(jīng)過修正后轉(zhuǎn)換為機(jī)器人關(guān)節(jié)空間下的參考軌跡θr，輸入至設(shè)計好的位置內(nèi)環(huán)，保證在力跟蹤穩(wěn)定的情況下，提高機(jī)器人的位置控制精度。

2 機(jī)器人位置控制系統(tǒng)設(shè)計

基于動力學(xué)模型設(shè)計了改進(jìn)粒子群優(yōu)化模糊PD控制算法，使電梯導(dǎo)軌校準(zhǔn)機(jī)器人的實(shí)際關(guān)節(jié)轉(zhuǎn)角漸近收斂到期望關(guān)節(jié)轉(zhuǎn)角，最終實(shí)現(xiàn)機(jī)器人的位置控制。

2.1 模糊PD+前饋控制器設(shè)計

以課題組自主設(shè)計的5軸關(guān)節(jié)型電梯導(dǎo)軌校準(zhǔn)機(jī)器人為研究對象，由拉格朗日方程推導(dǎo)出機(jī)器人的動力學(xué)模型為

(1)

考慮到對各關(guān)節(jié)獨(dú)立地使用PD控制這種線性反饋控制律，可以保證系統(tǒng)的漸進(jìn)穩(wěn)定性，因此控制器中引入PD控制。獨(dú)立的PD控制率為

(2)

如果僅采用前饋控制，由于得不到反饋，受到擾動時關(guān)節(jié)轉(zhuǎn)角一直會有跟隨誤差出現(xiàn)，僅采用PD控制則平衡不了系統(tǒng)產(chǎn)生的非線性誤差[3]，結(jié)合式(1)和式(2)，得出此時機(jī)器人的控制方程為

(3)

(4)

(5)

kP0和kD0通過粒子群優(yōu)化算法獲得，且都為PD控制器中的初始參數(shù)；ΔkP和ΔkD為模糊控制器輸出的參數(shù)修整值。

2.2 模糊化和模糊控制規(guī)則

2.2.1 輸入變量及其模糊化

2.2.2 輸出變量及其模糊化

模糊控制器的輸出變量為比例增益的變化量ΔkP和微分增益的變化量ΔkD，量化后的論域分別取為[0,400]和[0,100]。輸出變量在相應(yīng)的論域上的參考模糊集定義為：NB(負(fù)大)、NM(負(fù)中)、NS(負(fù)小)、ZO(零)、PS(正小)、PM(正中)、PB(正大)。

2.2.3 模糊控制規(guī)則

模糊控制算法在PD+前饋控制器的基礎(chǔ)上進(jìn)行參數(shù)調(diào)整，并且基于整定原則以及專家經(jīng)驗(yàn)[4]，得到模糊控制器的輸出曲面如圖2所示。

圖2 模糊控制器輸出曲面

2.2.4 精確化計算

先按照模糊規(guī)則表進(jìn)行模糊決策，并通過重心法解模糊，最后通過式(6)確定機(jī)器人每個關(guān)節(jié)的比例增益和微分增益，填入ΔkP和ΔkD矩陣中。重心法計算式為[5]

(6)

z0為解模糊化后的準(zhǔn)確值；zi為模糊論域內(nèi)的值；uc(zi)為zi對應(yīng)模糊規(guī)則中的隸屬度值。

2.3 改進(jìn)的粒子群算法優(yōu)化模糊PD

傳統(tǒng)的模糊PD控制(FuzzyPD)的kP和kD的初值都是人為選擇的，完全依賴于經(jīng)驗(yàn)知識和模擬調(diào)試的參數(shù)，很難達(dá)到預(yù)期的效果[6]。因此，本文采用改進(jìn)的粒子群優(yōu)化算法對模糊PD進(jìn)行優(yōu)化，選取機(jī)器人每個關(guān)節(jié)的比例和微分增益的初值，之后確定kP和kD的初值。

基本的粒子群算法容易陷入局部最優(yōu)值，導(dǎo)致后面全局最優(yōu)解收斂過慢，因此，本文采用帶慣性權(quán)重的粒子群算法來提高粒子群的全局搜索能力，同時考慮模擬退火算法可以在初始溫度選擇和溫度下降合理的條件下，能以一定的概率突跳并收斂到全局最優(yōu)值，通過模擬退火算法改進(jìn)粒子群算法，可以提高粒子跳出局部最優(yōu)解的概率。

PsoFuzzyPD控制過程由初始化粒子群、適應(yīng)度函數(shù)、位置和速度更新以及模擬退火過程7個步驟組成。優(yōu)化步驟流程如圖3所示。

a.對每個粒子的位置x和速度v進(jìn)行隨機(jī)初始化，粒子數(shù)本文取30。

b.計算每個粒子的當(dāng)前適應(yīng)度值ffit(xi)，本文選擇的適應(yīng)度函數(shù)的表達(dá)式取誤差絕對值積分型，表達(dá)式為

(7)

e(t)為t時刻機(jī)器人關(guān)節(jié)空間下，期望關(guān)節(jié)角度與實(shí)際關(guān)節(jié)角度的偏差；T為完成一次仿真花費(fèi)的總時間。

c.計算個體的最優(yōu)值(Pbest)和群體的最優(yōu)值(gbest)，每次迭代得到適應(yīng)度值最優(yōu)的個體極值存儲在群體最優(yōu)之中。

d.根據(jù)個體最優(yōu)值和群體最優(yōu)值，按照式(8)和式(9)對個體的速度vi和位置xi進(jìn)行更新。

(8)

(9)

c1和c2為學(xué)習(xí)因子，通常為0到2之間；r1和r2為均勻分布的2個獨(dú)立隨機(jī)函數(shù)，隨機(jī)生成0～1之間的實(shí)數(shù)；w為慣性權(quán)重因子。

e.判斷粒子群優(yōu)化過程是否達(dá)到全局最優(yōu)或達(dá)到最大迭代次數(shù)，如果沒有達(dá)到，重新返回步驟b，達(dá)到則執(zhí)行模擬退火操作。

f.判斷是否達(dá)到最低溫度要求，若沒有達(dá)到，則按照式(12)進(jìn)行降溫操作，根據(jù)式(10)進(jìn)行溫度初始化操作。若達(dá)到則直接輸出結(jié)果。

g.隨機(jī)生1個新的粒子種群，并且按照式(11)計算新解的接受概率，同時引入1個隨機(jī)數(shù)，用來和新解的接受概率P比較，判斷是否接受惡劣解，有利于增強(qiáng)算法跳出局部最優(yōu)的能力，并代入式(8)和式(9)進(jìn)行粒子的位置x和速度v的更新。

溫度初始化公式為

Ttemp=-ffit(xi)/log0.2

(10)

每個粒子新解的接受概率P為

(11)

ffit(xi)和ffit(xi+1)為當(dāng)前溫度下目前粒子適應(yīng)度值和新的粒子適應(yīng)度值。

降溫操作的公式為

Ttemp=γTtemp

(12)

本算法中γ取0.5。

圖3 改進(jìn)的粒子群算法流程

3 機(jī)器人基于位置的阻抗控制模型的建立

3.1 機(jī)器人阻抗控制模型

課題組設(shè)計的電梯導(dǎo)軌校準(zhǔn)機(jī)器人調(diào)整導(dǎo)軌時，采用阻抗控制方法，將導(dǎo)軌與機(jī)器人末端夾持器的關(guān)系模型等效為質(zhì)量-彈簧-阻尼物理系統(tǒng)，如圖4所示。xe為導(dǎo)軌實(shí)際位置，Ke為環(huán)境剛度參數(shù)n×n維正定對角陣，x為機(jī)器人末端夾持器的實(shí)際位移，xr為末端夾持器的期望位置。二者之間產(chǎn)生的交互力Fext[7]為

(13)

z為機(jī)器人末端夾持器的實(shí)際位置；ze為末端夾持器夾持導(dǎo)軌時與環(huán)境接觸時的初始實(shí)際位置，并且z和ze都是n×1維的矢量。

圖4 機(jī)器人末端執(zhí)行器與導(dǎo)軌之間的物理模型

機(jī)器人末端執(zhí)行器與導(dǎo)軌之間的物理模型等效為

Fr-Fext

(14)

Fr為末端期望力；Md為慣性參數(shù)；Bd為阻尼參數(shù)；Kd為剛度系數(shù)。

阻抗控制的原理是將需要控制的末端交互力用等效阻抗代替，當(dāng)電梯導(dǎo)軌校準(zhǔn)機(jī)器人調(diào)整導(dǎo)軌，與導(dǎo)軌之間存在交互力時，接觸處將產(chǎn)生阻抗力進(jìn)行補(bǔ)償，從而實(shí)現(xiàn)位置和力的動態(tài)交互與控制[8]。

在阻抗控制過程中，機(jī)器人從自由空間進(jìn)入到受限空間時，機(jī)器人末端交互力需要實(shí)現(xiàn)過渡平滑，力誤差響應(yīng)快，超調(diào)量小。此時機(jī)器人的期望阻抗參數(shù)應(yīng)滿足[9]

(15)

當(dāng)機(jī)器人運(yùn)動實(shí)際位置滿足阻抗控制需求時，通過末端實(shí)際接觸力進(jìn)行判別，并通過式(16)切換至阻抗控制，切換條件中的接觸力閾值可以根據(jù)實(shí)際情況設(shè)定。

(16)

Fw為接觸力閾值。

3.2 基于位置的阻抗控制方法

通過計算機(jī)器人與環(huán)境間的交互力，并結(jié)合式(14)的物理模型生成位置修正量，修正機(jī)器人的期望運(yùn)動軌跡，內(nèi)環(huán)的位置控制器則按照修正后的軌跡進(jìn)行運(yùn)動[10]。其中，反饋?zhàn)饔昧e等于末端期望力Fr減去末端實(shí)際力Fext，位置修正量的頻域表達(dá)式為

(17)

通過式(17)生成機(jī)器人的位置修正量e。位置修正量e對參考軌跡Zr修正后，生成機(jī)器人的控制軌跡Zd。當(dāng)機(jī)器人和環(huán)境沒有接觸時，機(jī)器人正常運(yùn)動，位置修正量e= 0，此時Zd=Zr。當(dāng)機(jī)器人與環(huán)境進(jìn)行交互時，此時e=Zd-Zr。

3.3 末端期望力自校正方法

機(jī)器人的末端期望力在導(dǎo)軌實(shí)際校準(zhǔn)過程中，并不能提前知曉，因此，需要根據(jù)校準(zhǔn)精度要求對機(jī)器人末端期望力實(shí)時調(diào)節(jié)，并且限制校準(zhǔn)力的大小，所以建立末端期望校準(zhǔn)力的模型為

(18)

KP為比例增益對角矩陣；KI為積分增益對角矩陣；KD為微分增益對角矩陣；Frmax為允許的最大期望力大小，可根據(jù)實(shí)際情況調(diào)整。

末端期望力經(jīng)過阻抗控制環(huán)節(jié)，使末端校準(zhǔn)力穩(wěn)定可控，并且根據(jù)機(jī)器人末端位置誤差實(shí)時調(diào)整校準(zhǔn)力的大小，以保證導(dǎo)軌校準(zhǔn)精度。

4 仿真實(shí)驗(yàn)分析

為了驗(yàn)證本文所提出的基于粒子群-模糊PD的電梯導(dǎo)軌校準(zhǔn)機(jī)器人力位控制策略的有效性和可靠性，將利用MATLAB的Simulink仿真模塊進(jìn)行模擬。仿真對象為本課題組自主設(shè)計的5軸關(guān)節(jié)型電梯導(dǎo)軌校準(zhǔn)機(jī)器人，電梯導(dǎo)軌校準(zhǔn)機(jī)器人連桿參數(shù)如表1所示,ai為連桿長度，αi為連桿扭角，di為連桿偏置，θi為關(guān)節(jié)轉(zhuǎn)角，其中，連桿參數(shù)為：a1= 0.14 m，a2= 0.35 m，a3= 0.32 m，d1= 0.134 m，d5= 0.108 m。

表1 電梯導(dǎo)軌校準(zhǔn)機(jī)器人連桿參數(shù)

根據(jù)阻抗控制器的參數(shù)分析，并且結(jié)合式(16)和電梯導(dǎo)軌校準(zhǔn)機(jī)器人的實(shí)際控制模型，取慣性參數(shù)Md=diag[1,1]，阻尼參數(shù)Kd=diag[300,1 300]，剛度參數(shù)Bd=diag[0.5,0.5]，環(huán)境剛度Ke=diag[10 000,100 000] N/m。

設(shè)定機(jī)器人末端執(zhí)行器的笛卡爾初始坐標(biāo)為[x,y,z] = [0.81,0,0.242]，導(dǎo)軌的坐標(biāo)為[x,y,z] = [0.7,0.025 8,0.242]，導(dǎo)軌校準(zhǔn)終止坐標(biāo)為[x,y,z] = [0.682,0.03,0.242]，單位為m，在笛卡爾空間中進(jìn)行軌跡規(guī)劃。并且結(jié)合實(shí)際情況考慮機(jī)器人末端執(zhí)行器校準(zhǔn)導(dǎo)軌時，導(dǎo)軌會在x方向和y方向上受到導(dǎo)軌支架的限制，所以既需要防止因?qū)к壷Ъ軐κ芰^大，固定導(dǎo)軌的支架出現(xiàn)斷裂、導(dǎo)軌產(chǎn)生彈塑性形變而損壞導(dǎo)軌等安全問題，又需要穩(wěn)定地控制導(dǎo)軌校準(zhǔn)力，防止校準(zhǔn)力波動過大導(dǎo)致機(jī)器人的晃動影響校準(zhǔn)精度，所以在導(dǎo)軌的x和y方向進(jìn)行力控，機(jī)器人末端的接觸力由式(13)產(chǎn)生，末端期望接觸力由式(18)產(chǎn)生，并模擬機(jī)器人校準(zhǔn)電梯導(dǎo)軌時的運(yùn)動軌跡。

仿真步長為變步長，總時長為10 s，機(jī)器人在自由空間中運(yùn)動8.6 s接觸到導(dǎo)軌后，開始調(diào)整導(dǎo)軌。如圖5和圖6所示，通過對比NormalPD、FuzzyPD和PsoFuzzyPD可以發(fā)現(xiàn)，采用本文方法時，機(jī)器人末端的導(dǎo)軌校準(zhǔn)力和機(jī)器人末端x和y方向的位移控制更為平滑，無波動，超調(diào)更小，進(jìn)入穩(wěn)態(tài)的時間更快，震蕩次數(shù)更少，從而使得導(dǎo)軌調(diào)節(jié)時間更短，可以防止在實(shí)際調(diào)整過程中出現(xiàn)的安全問題，并且可以精確地控制調(diào)整精度。

圖5 導(dǎo)軌校準(zhǔn)力變化曲線

如圖7所示，模擬仿真的機(jī)器人關(guān)節(jié)誤差采樣周期取0.002 s，并在機(jī)器人的運(yùn)動規(guī)劃過程中得到各關(guān)節(jié)的軌跡跟蹤偏差，在8.6 s接觸到導(dǎo)軌后，關(guān)節(jié)誤差波動明顯，3種控制器均可快速調(diào)節(jié)誤差，表現(xiàn)有不錯的快速響應(yīng)性，通過對比NormalPD、FuzzyPD和PsoFuzzyPD可以發(fā)現(xiàn)，本文設(shè)計的力位控制器利用PsoFuzzyPD對PD的2個參數(shù)進(jìn)行自動尋優(yōu)和自調(diào)整，相比于NormalPD和FuzzyPD在軌跡跟蹤偏差上有最小的上升時間和最小的超調(diào)量。進(jìn)一步采用式(7)可計算得到機(jī)器人末端關(guān)節(jié)在關(guān)節(jié)空間中的目標(biāo)函數(shù)值如表2所示，并用該值表征機(jī)器人末端關(guān)節(jié)的仿真軌跡與期望軌跡的偏差[11]，可見采用PsoFuzzyPD的控制器具有更小的軌跡跟蹤誤差，在笛卡爾空間中軌跡跟蹤偏差分別比FuzzyPD控制器和NormalPD控制器小42.92%和47.39%。

圖6 機(jī)器人末端夾持器x和y方向位移

表2 軌跡偏差

仿真結(jié)果表明，采用本文方法進(jìn)行仿真時，末端執(zhí)行器實(shí)際終止坐標(biāo)為[x,y,z] = [0.682 2,0.029 5,0.242 0]，單位為m，在x方向上有0.2 mm的位置誤差，在y方向上有0.5 mm的位置誤差，符合位置控制精度要求。并且校準(zhǔn)力控制平滑，無波動，在x方向上最終穩(wěn)定在178.33 N，在y方向上最終穩(wěn)定在373.59 N，均未超過最大校準(zhǔn)力限制。

圖7 關(guān)節(jié)空間軌跡跟蹤偏差

5 結(jié)束語

針對電梯導(dǎo)軌校準(zhǔn)機(jī)器人的位置/力控制問題，本文提出了一種基于改進(jìn)粒子群算法和模糊PD的電梯導(dǎo)軌校準(zhǔn)機(jī)器人力位控制方法，通過該控制方法可使電梯導(dǎo)軌垂直度的重復(fù)調(diào)整精度達(dá)到±0.5 mm，提高電梯導(dǎo)軌校準(zhǔn)力的控制精度和穩(wěn)定性。首先建立了電梯導(dǎo)軌校準(zhǔn)機(jī)器人的整體控制框架，之后設(shè)計了改進(jìn)的粒子群優(yōu)化模糊PD+前饋控制器作為機(jī)器人的位置控制器，可以自適應(yīng)不同的接觸環(huán)境和工作條件，自動改變PD控制器的參數(shù)，提高位置控制精度，最后設(shè)計了阻抗控制外環(huán)，對末端期望力進(jìn)行自適應(yīng)調(diào)整，實(shí)現(xiàn)平滑穩(wěn)定的校準(zhǔn)力控制。通過現(xiàn)場實(shí)際使用反饋，該控制系統(tǒng)運(yùn)行穩(wěn)定、實(shí)用性強(qiáng)、滿足用戶對導(dǎo)軌垂直度精確調(diào)整和校準(zhǔn)力大小控制穩(wěn)定的使用需求，具有一定的應(yīng)用推廣價值。