基于腕力傳感器的繩驅(qū)動(dòng)擬人臂的力/位混合控制研究

唐振山，陳偉海

（北京航空航天大學(xué)自動(dòng)化科學(xué)與電氣工程學(xué)院，北京 100191）

0 引 言

繩驅(qū)動(dòng)擬人臂樣機(jī)是一種串并聯(lián)混合結(jié)構(gòu)，樣機(jī)由并聯(lián)驅(qū)動(dòng)肩關(guān)節(jié)、鍵驅(qū)動(dòng)肘關(guān)節(jié)和并聯(lián)驅(qū)動(dòng)腕關(guān)節(jié)組成[1]，采用鋼絲繩代替連桿作為驅(qū)動(dòng)元件并將驅(qū)動(dòng)電機(jī)安裝于基座上。它結(jié)合了并聯(lián)結(jié)構(gòu)高剛度、高精度、高負(fù)載能力和繩驅(qū)動(dòng)結(jié)構(gòu)重量輕、靈活性好的優(yōu)點(diǎn)，會(huì)在廣泛的領(lǐng)域擁有較好的應(yīng)用前景。但由于繩索固有的柔性[2]，機(jī)構(gòu)安裝的初始定位誤差、電機(jī)卷?yè)P(yáng)機(jī)構(gòu)纏繞誤差的存在，導(dǎo)致這種非直接驅(qū)動(dòng)系統(tǒng)在任務(wù)空間的開環(huán)精度和剛度較低，從而使其在微操作或精度要求較高場(chǎng)合的應(yīng)用受到了限制。本文采用3個(gè)相交于一點(diǎn)的單軸機(jī)構(gòu)來(lái)實(shí)現(xiàn)自由度球關(guān)節(jié)的功能，通過(guò)安裝在關(guān)節(jié)處的角度傳感器，實(shí)現(xiàn)高精度的位置控制。

機(jī)器人在從事裝配、打磨、拋光及輪廓跟蹤等任務(wù)時(shí)，不但需要高精度的位置控制，還需要實(shí)時(shí)的控制其與環(huán)境之間的接觸力，本文主要是在阻抗控制[3]算法的基礎(chǔ)上進(jìn)行位置控制和力控制。

1 運(yùn)動(dòng)學(xué)模型分析

繩驅(qū)動(dòng)擬人臂的示意圖如圖1所示，由3個(gè)關(guān)節(jié)組成，即3-DOF的并聯(lián)肩關(guān)節(jié)、1-DOF的肘關(guān)節(jié)、3-DOF的并聯(lián)腕關(guān)節(jié)。建立如圖1的坐標(biāo)系，基坐標(biāo)系為{Ko}，肩關(guān)節(jié)坐標(biāo)系為{Ks}，肘關(guān)節(jié)坐標(biāo)系{Ke}，腕關(guān)節(jié)坐標(biāo)系為{Kw}，末端操作手坐標(biāo)系{KT}。

圖1 七自由度擬人臂示意圖

機(jī)械臂的前向運(yùn)動(dòng)學(xué)公式為

式中：{TOS}，{TSE}，{TEW}，{TWF}——肩關(guān)節(jié)坐標(biāo)系相對(duì)基坐標(biāo)系，肘關(guān)節(jié)坐標(biāo)系相對(duì)肩關(guān)節(jié)坐標(biāo)系，腕關(guān)節(jié)坐標(biāo)系相對(duì)肘關(guān)節(jié)坐標(biāo)系及工具坐標(biāo)系相對(duì)腕關(guān)節(jié)坐標(biāo)系的變化矩陣，它們分別如式（2）～式（5）所示

式中：po，pa，pb，pc——關(guān)節(jié)之間連桿的長(zhǎng)度；

q1，q2，q3——肩關(guān)節(jié)姿態(tài)的歐拉角；

q4——肘關(guān)節(jié)的角度；

q5，q6，q7——腕關(guān)節(jié)姿態(tài)的歐拉角；

s1，s2，s3，s4，s5，s6，s7——7個(gè)關(guān)節(jié)角對(duì)應(yīng)的7個(gè)旋量[4]。

2 阻抗模型分析

阻抗控制中的阻抗包括操作機(jī)物理上內(nèi)在的阻抗和采用主動(dòng)控制帶來(lái)的阻抗兩部分。操作機(jī)物理上內(nèi)在的阻抗是不變的，阻抗控制的目的就是通過(guò)選用主動(dòng)控制參數(shù)來(lái)實(shí)現(xiàn)理想的目標(biāo)阻抗。阻抗控制中，阻抗可以為任意的函數(shù)形式：f=Z（x），但考慮到實(shí)現(xiàn)的可能，采用式（6）的2階線性阻抗[5]

Md∈R3×3，Bd∈R3×3——系統(tǒng)的理想質(zhì)量矩陣和阻尼矩陣；

F∈R3——工具與環(huán)境的接觸力向量；

Kf∈R3×3——力反饋增益矩陣；，F(xiàn)d——末端操作手理想的速度向量，加速度向量和作用力向量；

Md，Bd，Kf均設(shè)定為正定的對(duì)角陣。

由式（7）可解得

X=e-Md-1BdtX（0）+

為了進(jìn)行實(shí)際控制，把式（8）進(jìn)行離散化處理，采樣時(shí)間為 Δt，假設(shè) Md，Bd，Kf，F(xiàn) 和 Fd在采樣間隔 Δt·（k-1）≤t＜Δt·k 是連續(xù)的，令，則式（8）可變換為

式中：Ki∈R3×3——正定對(duì)角的積分增益矩陣。

3 軌跡規(guī)劃

3.1 標(biāo)準(zhǔn)環(huán)境的軌跡規(guī)劃

當(dāng)環(huán)境能精確建模時(shí)，如圖2所示，采用解析的方式得到環(huán)境的信息，從而得到機(jī)器人的準(zhǔn)確參考軌跡。例如末端跟蹤的軌跡為圖2所示的X_Y平面中圓心為B的圓弧時(shí)，令機(jī)器人末端操作手的法向量平行于Z軸，即垂直于X_Y平面，用式（11）參數(shù)方程進(jìn)行軌跡建模

圖2 X-Y平面上的圓弧軌跡

式中：R——圓弧半徑；

θ——圓弧上點(diǎn)對(duì)應(yīng)的角度值。

假設(shè)機(jī)器人末端執(zhí)行器做圓弧運(yùn)動(dòng)時(shí)，角速度曲線為加速、勻速、減速的梯形曲線，加速、減速時(shí)間為Ta，總時(shí)間為T，則最大角速度為

式中：θ0——整個(gè)圓弧對(duì)應(yīng)的角度。

最大角加速度為

可得角速度方程為

為了進(jìn)行數(shù)字控制，把式（14）離散化，設(shè)采樣時(shí)間為 Δt，令

軌跡方程變換為

式中：n——采樣點(diǎn)數(shù)。

可得末端的位置向量為

設(shè)末端的姿態(tài)保持不變一直為Ra∈R3×3，則末端的位置可以表示為

3.2 未知環(huán)境軌跡規(guī)劃

當(dāng)機(jī)器人與一些環(huán)境比如平面、圓弧面等標(biāo)準(zhǔn)環(huán)境接觸時(shí)，環(huán)境一般能被精確建模，環(huán)境的每一點(diǎn)的切向和法向能通過(guò)理論計(jì)算得到；但當(dāng)機(jī)器人與未知環(huán)境發(fā)生接觸時(shí)，例如對(duì)不規(guī)則物體的表面打磨、輪廓跟蹤等，機(jī)器人末端的參考運(yùn)動(dòng)軌跡并不能提前給定，這時(shí)就需要通過(guò)力、位置等反饋信息進(jìn)行在線軌跡規(guī)劃，根據(jù)當(dāng)前點(diǎn)的位置和與環(huán)境作用力進(jìn)行在線計(jì)算得到下一點(diǎn)的參考值[6]。

當(dāng)機(jī)器人與未知環(huán)境接觸時(shí)，如圖3所示，F(xiàn)x與Fy為安裝機(jī)器人末端的多維力傳感器測(cè)得的與環(huán)境之間的接觸力，p為垂直于運(yùn)動(dòng)方向的單位法向量，w和t分別為工具與環(huán)境接觸點(diǎn)處的單位法向量和切向量，忽略工具與環(huán)境之間的摩擦[7]。

圖3 不確定性環(huán)境下的軌跡模型

w和t可由式（19）得出

由于安裝在末端的力傳感器上工具坐標(biāo)系為動(dòng)坐標(biāo)系，從而測(cè)得的多維力信息需要一定的矩陣變換才能得到未知環(huán)境的信息。

式中：Re——當(dāng)前點(diǎn)的末端的姿態(tài)矩陣；

F′——力傳感器測(cè)得的力向量；

F——相對(duì)于全局坐標(biāo)系的力向量。

當(dāng)末端操作手運(yùn)動(dòng)到 P（k）點(diǎn)時(shí)，P（k+1）點(diǎn)的位置信息可通過(guò)式（21）計(jì)算得到

4 位置控制算法

擬人臂在任務(wù)空間的定位操作是實(shí)現(xiàn)擬人臂各種復(fù)雜運(yùn)動(dòng)的基礎(chǔ)，只有樣機(jī)的位置控制精度達(dá)到要求，軌跡跟蹤、微動(dòng)等任務(wù)才能快速順利完成。實(shí)際中樣機(jī)模型參數(shù)的準(zhǔn)確性很大程度上影響了末端執(zhí)行器的定位精度，樣機(jī)在加工和裝配過(guò)程中，間隙、回差、非垂直連接等因素，加之繩索本身的柔性，都導(dǎo)致系統(tǒng)精度的降低。為了消除各種不確定因素帶來(lái)的誤差，采用關(guān)節(jié)角閉環(huán)控制的方式，這樣可以不必對(duì)每個(gè)誤差源進(jìn)行建模并補(bǔ)償消除，直接通過(guò)PID控制進(jìn)行每個(gè)關(guān)節(jié)的控制。樣機(jī)由3個(gè)關(guān)節(jié)構(gòu)成：2個(gè)三自由度的球關(guān)節(jié)和1個(gè)一自由度的肘關(guān)節(jié)，球關(guān)節(jié)是1個(gè)多輸入多輸出的耦合結(jié)構(gòu)，底層驅(qū)動(dòng)由4個(gè)伺服電機(jī)完成。伺服電機(jī)的一個(gè)缺點(diǎn)是輸入命令為電機(jī)的相對(duì)或絕對(duì)位置，需要根據(jù)關(guān)節(jié)誤差實(shí)時(shí)的改變每個(gè)電機(jī)的最終位置，但由于耦合關(guān)節(jié)非線性的因素及繩索彈性的影響，并不能確定電機(jī)的最終位置，這樣傳統(tǒng)的PID控制器不能順利完成，為此提出了一種改進(jìn)的PD算法。

底層電機(jī)控制采用了一種可以在電機(jī)運(yùn)動(dòng)中改變目標(biāo)位置的技術(shù)，如圖4所示，這種技術(shù)避免了電機(jī)不同運(yùn)動(dòng)之間的停頓（圖5），使運(yùn)動(dòng)速度更加平滑。這種技術(shù)是首先給定電機(jī)一個(gè)虛擬目標(biāo)位置點(diǎn)，在沒有到達(dá)這個(gè)位置點(diǎn)的過(guò)程中，實(shí)時(shí)監(jiān)測(cè)關(guān)節(jié)處編碼器的反饋值，通過(guò)解耦計(jì)算實(shí)時(shí)地修正這個(gè)虛擬目標(biāo)點(diǎn)，采樣時(shí)間為10ms，這樣在一個(gè)運(yùn)動(dòng)過(guò)程中，一般為幾百毫秒到幾秒，足以把個(gè)各種誤差消除到理想范圍內(nèi)，之后的實(shí)驗(yàn)也表明算法的有效性。

圖4 改進(jìn)的PD算法示意圖

圖5 傳統(tǒng)的控制算法示意圖

5 實(shí) 驗(yàn)

為了得到較好的控制效果，運(yùn)用類似于生物驅(qū)動(dòng)機(jī)制的分級(jí)控制系統(tǒng)作為控制器，分級(jí)控制系統(tǒng)框圖如圖6所示。

圖6 控制系統(tǒng)整體框圖

本文在樣機(jī)控制系統(tǒng)的基礎(chǔ)上進(jìn)行實(shí)驗(yàn)來(lái)驗(yàn)證算法的正確性：

實(shí)驗(yàn)一：擬人臂各個(gè)關(guān)節(jié)從初始位置運(yùn)動(dòng)到給定的角度，設(shè)定值如下：肩關(guān)節(jié)的歐拉角Z_1、Y_1、X_1 分別為 10°、15°、20°，肘關(guān)節(jié)角度 M 為 60°，腕關(guān)節(jié)的歐拉角為 15°、15°、15°，運(yùn)動(dòng)過(guò)程中實(shí)時(shí)監(jiān)測(cè)關(guān)節(jié)角的角度值并繪制成曲線，如圖7，圖8，圖9所示，可見機(jī)器人各關(guān)節(jié)定位精度均達(dá)到碼盤的分辨率0.18°以內(nèi)，采用上面閉環(huán)控制算法得到的精度是滿足系統(tǒng)要求的。

實(shí)驗(yàn)二：使機(jī)器人在X_Y平面內(nèi)繪制一條規(guī)劃好的曲線，曲線跟蹤算法采用的是自學(xué)習(xí)算法，結(jié)果如圖10所示，可以看出機(jī)械臂對(duì)于空間規(guī)劃的曲線具有很高的跟蹤能力，位置精度較高[8]。

圖7 肩關(guān)節(jié)角度響應(yīng)曲線

圖8 肘關(guān)節(jié)角度響應(yīng)曲線

圖9 腕關(guān)節(jié)角度響應(yīng)曲線

6 仿真

采用式（22）計(jì)算力傳感器信號(hào)：

圖10 圓弧軌跡

其中noise為傳感器噪聲，為與工具末端與環(huán)境z坐標(biāo)的差值，設(shè)定環(huán)境剛度為Km=40N/mm，反饋力增益矩陣：積分矩陣取環(huán)境作用力Fd設(shè)定為Fd=[0 0 10]N，采用阻抗模型進(jìn)行控制仿真得到圖11與圖12中的曲線。

從圖12中的力響應(yīng)曲線可以看出，阻抗控制算法能快速地實(shí)現(xiàn)力跟蹤并且具有較高的跟蹤精度，Z方向上的位置在參考位置的基礎(chǔ)上產(chǎn)生一定的偏差來(lái)滿足接觸力控制的要求，通過(guò)位置和力之間的這種動(dòng)態(tài)關(guān)系調(diào)節(jié)，實(shí)現(xiàn)了系統(tǒng)的力/位混合控制。

7 結(jié)束語(yǔ)

（1）在一種具有關(guān)節(jié)角反饋的繩驅(qū)動(dòng)擬人臂樣機(jī)的基礎(chǔ)上，利用關(guān)節(jié)角反饋信息提出一種適合耦合球關(guān)節(jié)的閉環(huán)控制算法，克服了繩驅(qū)動(dòng)技術(shù)多種誤差源不能建模的問(wèn)題，提高了整機(jī)的運(yùn)動(dòng)精度。

（2）結(jié)合多維力傳感器提出了一種基于阻抗控制的力控制算法，并在此基礎(chǔ)上進(jìn)行了仿真實(shí)驗(yàn)，驗(yàn)證了阻抗模型的正確性。

（3）對(duì)機(jī)器人的兩種不同的任務(wù)環(huán)境進(jìn)行分析，并利用多維力傳感器對(duì)環(huán)境信息進(jìn)行估計(jì)與建模，在此基礎(chǔ)上對(duì)所要實(shí)施的任務(wù)進(jìn)行了軌跡規(guī)劃。

（4）采用基于PC+PMAC分級(jí)控制器系統(tǒng)實(shí)現(xiàn)控制系統(tǒng)，有效利用了PC資源豐富、開發(fā)調(diào)試和運(yùn)動(dòng)控制器實(shí)時(shí)運(yùn)算能力強(qiáng)的優(yōu)點(diǎn)，同時(shí)系統(tǒng)具有很好的開放性和可擴(kuò)展性。

圖11 接觸力響應(yīng)曲線

圖12 Z方向位置響應(yīng)曲線

[1]陳泉柱.繩驅(qū)動(dòng)擬人臂機(jī)器人的機(jī)構(gòu)設(shè)計(jì)與運(yùn)動(dòng)學(xué)研究[D].北京：北京航空航天大學(xué)，2006.

[2]王洪光，趙明揚(yáng)，郭立新，等.一種柔索驅(qū)動(dòng)并聯(lián)機(jī)構(gòu)及其運(yùn)動(dòng)學(xué)分析[J].機(jī)器人，2002，24（7）：634-636.

[3]Hogan N.Impedance control，an approach to manipulation[C]∥American Control Conference.1984：304-314.

[4]John J.Craig，贠超.機(jī)器人學(xué)導(dǎo)論[M].3版.北京：機(jī)械工業(yè)出版社，2006：136-150.

[5]Fusaomi Nagata， Tetsuo Hase，Zenku Haga，et al.Watanabe CAD/CAM based position/force controller for a mold polishing robot[J].Mechatronics，2007（17）：207-216.

[6]喬兵，吳洪濤，朱建英，等.面向位控機(jī)器人的力/位混合控制[J].機(jī)器人，1999，21（7）：7.

[7]Yu K G，Kieffer J.Robotic force velocity control for followingunknown contours，ofgranularmaterials[J].Control Engineering Practice，1999（7）：1249-1256.

[8]Burdet E，Rey L，Codourey A.A trivial and efficient learning method for motion and force control[J].Engineering Applications of Artificial Intelligence，2001（14）：487-496.