程 楊 潘尚峰 孫江宏

（清華大學(xué)機(jī)械工程系，北京 100086）

腦卒中具有高致殘率及高復(fù)發(fā)率的特點(diǎn)，約3/4的存活者存在不同程度的殘疾，且5 年復(fù)發(fā)率高達(dá)41%[1]，給家庭和社會帶來極大的負(fù)擔(dān)。積極規(guī)范的康復(fù)治療可以在最大程度上改善腦卒中預(yù)后，提高患者的生活質(zhì)量[2]?？祻?fù)機(jī)器人可以部分解決我國目前康復(fù)醫(yī)療資源不足的問題，它們大多儲存內(nèi)置一些標(biāo)準(zhǔn)的康復(fù)動作庫，通過被動、主動康復(fù)等模式，輔助病人康復(fù)治療。但由于不同病人、不同階段的特異性，康復(fù)醫(yī)師會對此開具具有針對性的“運(yùn)動處方[3]”，而這些動作不存在于標(biāo)準(zhǔn)康復(fù)動作庫，所以開發(fā)示教訓(xùn)練十分必要。利用示教模式，可以將康復(fù)機(jī)器人的康復(fù)訓(xùn)練與康復(fù)醫(yī)師的臨床經(jīng)驗(yàn)相結(jié)合，從而針對具體病人的實(shí)際情況，提供科學(xué)、高效的康復(fù)訓(xùn)練計劃，顯著改善康復(fù)治療的效果[4]。

與采用人體動作捕捉設(shè)備[5]、雙目視覺測距[6]等方式采集康復(fù)示教動作相比，穿戴拖動示教不需要額外的采集設(shè)備，能夠降低系統(tǒng)的復(fù)雜性?？祻?fù)醫(yī)師穿戴康復(fù)機(jī)械臂進(jìn)行康復(fù)動作示教，機(jī)械臂在受控狀態(tài)下完成跟隨動作，在線保存軌跡數(shù)據(jù)，儲存康復(fù)動作，從而完成示教。在實(shí)際應(yīng)用時通常采用Hogan N[7]提出的阻抗控制方法，使康復(fù)機(jī)械臂具有柔順特性，完成這一人機(jī)交互過程。

康復(fù)示教過程是動態(tài)時變的，固定參數(shù)的導(dǎo)納控制模型難以充分反應(yīng)人機(jī)交互的特征，且隨著關(guān)節(jié)活動度的不同，人體關(guān)節(jié)的剛度也會發(fā)生改變。為保障拖動示教過程的安全性及跟隨性，機(jī)器人需要具有可變的柔順性[8]。變導(dǎo)納控制算法有模糊變導(dǎo)納、自適應(yīng)變導(dǎo)納以及智能控制策略，如結(jié)合強(qiáng)化學(xué)習(xí)、神經(jīng)網(wǎng)絡(luò)等方法進(jìn)行變導(dǎo)納控制[9]。付興[10]等利用余弦相似度，模糊辨識機(jī)器人的狀態(tài)，如啟動、加速、高速穩(wěn)定運(yùn)動及減速等，不同狀態(tài)配以不同的阻尼參數(shù)，并通過試驗(yàn)驗(yàn)證了所提出控制方法的有效性，但其運(yùn)動軌跡較為簡單，普適性較差。吳青聰[11]等利用模糊滑膜導(dǎo)納控制算法，改變系統(tǒng)導(dǎo)納參數(shù)，增強(qiáng)人機(jī)交互的柔順性，提高系統(tǒng)的舒適性。郝向陽[12]利用李雅普諾夫第二法構(gòu)建系統(tǒng)自適應(yīng)控制模塊，降低了拖動過程中的波動和干擾。杜志江[13]等提出基于模糊強(qiáng)化學(xué)習(xí)的變導(dǎo)納人機(jī)交互控制模型，結(jié)合個人操作因素，并運(yùn)用于微創(chuàng)外科手術(shù)機(jī)器人的姿態(tài)調(diào)整，取得一定的成果。王飛[14]等提出根據(jù)環(huán)境剛度變化在線調(diào)節(jié)導(dǎo)納阻尼參數(shù)的方法，使得機(jī)械臂牽引跟蹤軌跡更加平滑，但機(jī)械臂為3 自由度，動力學(xué)模型耦合度較低。

針對示教過程中康復(fù)機(jī)械臂由于柔順性不足導(dǎo)致的隨動效果不佳的問題，本文設(shè)計了在關(guān)節(jié)空間內(nèi)結(jié)合力矩微分項(xiàng)的導(dǎo)納控制器，并結(jié)合雙曲正切函數(shù)設(shè)計了改進(jìn)的變導(dǎo)納控制策略，提高機(jī)械臂的柔順性，以期望獲得良好的跟隨效果。

1 康復(fù)機(jī)械臂介紹

為實(shí)現(xiàn)穿戴康復(fù)示教的目的，本文采用的康復(fù)機(jī)械臂如圖1 所示。康復(fù)機(jī)械臂具有4 個主動旋轉(zhuǎn)關(guān)節(jié)，1～3 號關(guān)節(jié)由3 個旋轉(zhuǎn)軸相交且相差60°的電機(jī)組成，用于模擬人體肩關(guān)節(jié)的運(yùn)動自由度，4號關(guān)節(jié)用于模擬人體肘關(guān)節(jié)的自由度。這4 個自由度可實(shí)現(xiàn)人體上肢的肩部外旋/內(nèi)旋、外展/內(nèi)收、屈曲/伸展及肘部屈曲/伸展功能。每個電機(jī)的輸出端均通過輸出桿安裝一個BONGSHIN CDFS-100kgf拉壓力傳感器，用于間接測量人機(jī)交互力矩（圖2為以電機(jī)3 為例，拉壓力傳感器安裝示意）。

圖1 康復(fù)機(jī)械臂實(shí)物圖片

圖2 拉壓力傳感器安裝圖示

參照實(shí)際使用的材料及零部件尺寸建立康復(fù)機(jī)械臂的SolidWorks 模型，在肩心位置和每個自由度的旋轉(zhuǎn)中心沿電機(jī)輸出軸方向，建立了右手坐標(biāo)系，并將得到的標(biāo)準(zhǔn)Denavit-Hartenberg 參數(shù)匯總在表1 中。

表1 機(jī)械臂標(biāo)準(zhǔn)DH 參數(shù)表

2 導(dǎo)納控制系統(tǒng)設(shè)計

2.1 導(dǎo)納控制原理及改進(jìn)

結(jié)合本文采用的康復(fù)機(jī)械臂的特點(diǎn)，采用關(guān)節(jié)空間的導(dǎo)納控制。

根據(jù)導(dǎo)納控制的基本思想，將人體作為作業(yè)環(huán)境，則關(guān)節(jié)空間內(nèi)，人體上肢與康復(fù)外骨骼機(jī)械臂的導(dǎo)納模型如圖3 所示。其中，τi是關(guān)節(jié)處的人機(jī)交互力矩，Ji為轉(zhuǎn)動慣性系數(shù)，bi為阻尼系數(shù)，ki為剛度系數(shù)，ke為人機(jī)交互剛度系數(shù)。

圖3 轉(zhuǎn)角形式導(dǎo)納控制原理

則有：

在示教過程中，康復(fù)機(jī)械臂處于自由運(yùn)動狀態(tài)，故而設(shè)置ki=0,i=1,2,3,4 ，同時記Δ θi=θdi-θi，式（1）等效為：

為更好地改善人機(jī)交互效果、提高示教跟隨性及柔順性，引入人機(jī)交互力矩的微分項(xiàng)，從而增加導(dǎo)納控制器對人機(jī)交互力矩的敏感性并提高跟隨精度，對控制方程式（2）修正為：

對式（3）進(jìn)行拉氏變換，得到系統(tǒng)導(dǎo)納控制器的傳遞函數(shù)為：

對于整個系統(tǒng)而言，其示教控制算法流程框如圖4 所示。

圖4 示教控制算法流程框圖

圖4 中，位置內(nèi)環(huán)采用的是虛擬分解理論控制（virtual decomposition control,VDC）方法。使用者穿戴機(jī)械臂進(jìn)行康復(fù)動作，通過拉壓力傳感器采集到人機(jī)交互力，再結(jié)合輸出桿長、電機(jī)輸出轉(zhuǎn)矩及各關(guān)節(jié)重力補(bǔ)償矩，計算出實(shí)際人機(jī)交互力矩，輸入到導(dǎo)納控制器中，得到機(jī)械臂各關(guān)節(jié)下一周期期望位置。結(jié)合反饋的機(jī)械臂各關(guān)節(jié)現(xiàn)有角度，作為位置內(nèi)環(huán)的輸入，計算得到各關(guān)節(jié)所需的輸出扭矩，從而驅(qū)動機(jī)械臂到達(dá)所需位置。

2.2 基于雙曲正切函數(shù)的變參數(shù)導(dǎo)納控制算法

本文的變導(dǎo)納算法利用了雙曲正切函數(shù)（tanh），這是一種在神經(jīng)網(wǎng)絡(luò)中常見的標(biāo)準(zhǔn)激活函數(shù)，具有完全可微分、反對稱、對稱中心在原點(diǎn)的優(yōu)勢，保證了導(dǎo)納參數(shù)調(diào)整的平滑性。

在實(shí)際人機(jī)交互過程中，需要關(guān)注的是交互力矩的絕對值大小，與方向無關(guān)，且雙曲正切函數(shù)的值域是(-1,1)，為實(shí)現(xiàn)變導(dǎo)納控制，對雙曲正切函數(shù)進(jìn)行一定的變形處理。引入調(diào)整系數(shù)及對稱處理，得到新的調(diào)整函數(shù) tanh(x)：

tanh(x) 及 tanh(x)的函數(shù)圖像如圖5 所示。通過調(diào)整參數(shù)m、n的值，可以對 tanh(x)的值域及集中度進(jìn)行調(diào)整。m越大， tanh(x)的最小值越小。n越大，tanh(x)越集中，在0 附近的變化越快。

圖5 tanh(x)及tanh(x)的函數(shù)圖像

在具體的變參數(shù)導(dǎo)納過程中，轉(zhuǎn)動慣性系數(shù)J會同時引起系統(tǒng)固有頻率及阻尼的快速變化，其可調(diào)整的范圍有限，且會影響系統(tǒng)的整體穩(wěn)定性[8]。在本文中采用對阻尼系數(shù)b進(jìn)行調(diào)整。

當(dāng)人機(jī)交互力矩變大時，降低導(dǎo)納控制器的阻尼系數(shù)，可以使康復(fù)機(jī)械臂快速提供較好的順應(yīng)性，并改善系統(tǒng)的柔順性；當(dāng)人機(jī)交互力矩變小時，導(dǎo)納控制器的阻尼系數(shù)較大，可以保證系統(tǒng)的穩(wěn)定性及較高的位置控制精度。

利用調(diào)整函數(shù) tanh(x)，則有

其中：bmaxi為bi的最大值，m為范圍調(diào)整參數(shù)，可以調(diào)節(jié)bi的最小值，n為變化率調(diào)整參數(shù)，用于調(diào)節(jié)bi的變化率。

根據(jù)二階系統(tǒng)模型的特性，為保障系統(tǒng)的穩(wěn)定性，導(dǎo)納控制器還需要滿足以下3 個條件：

在示教訓(xùn)練中，ki=0;i=1,2,3,4，則上述條件中，條件1、2 顯然滿足，條件3 等效有：

所以m∈[0,1)，且需滿足式（8）。

3 仿真平臺搭建及結(jié)果分析

3.1 仿真平臺搭建

為驗(yàn)證提出的改進(jìn)變導(dǎo)納控制器的有效性，利用機(jī)械臂SolidWorks 模型（圖6），采用Adams 與MATLAB/Simulink 聯(lián)合仿真，進(jìn)行畫圓示教實(shí)驗(yàn)。

圖6 機(jī)械臂SolidWorks 模型

先將SolidWorks 中配置有質(zhì)量屬性的機(jī)械臂模型導(dǎo)入Adams 中，之后在4 個關(guān)節(jié)處創(chuàng)建旋轉(zhuǎn)運(yùn)動幅，添加對應(yīng)的旋轉(zhuǎn)運(yùn)動驅(qū)動，定義扭矩、角速度等矢量方向?？紤]到關(guān)節(jié)處摩擦力分析較為復(fù)雜，仿真時做了簡化處理。在Adams 中各旋轉(zhuǎn)運(yùn)動幅設(shè)置最大靜摩擦因數(shù)為0.3、動摩擦因數(shù)為0.1。

利用Adams 中的Controls 插件，定義系統(tǒng)輸入為4 個旋轉(zhuǎn)幅的驅(qū)動扭矩（圖7 中tourque1～4），輸出為4 個旋轉(zhuǎn)副的旋轉(zhuǎn)角度及角速度（圖7 中q1～q4_dot），導(dǎo)出為m 文件，并在Simulink 中打開，如圖7 所示。

圖7 Adams 導(dǎo)出的控制子模塊

為引入人機(jī)交互力矩信息，假設(shè)環(huán)境剛度為ke，即康復(fù)機(jī)械臂實(shí)際運(yùn)動角度與使用者的期望角度產(chǎn)生偏差時，為修正偏差，使用者在各對應(yīng)關(guān)節(jié)施加的交互力矩有

結(jié)合控制流程框圖4，搭建圖8 所示的Simulink圖，其中棕色部分為Adams 導(dǎo)出的機(jī)械臂m 文件，用于進(jìn)行動力學(xué)仿真；綠色的部分為系統(tǒng)的內(nèi)部位置環(huán)，采用虛擬分解理論進(jìn)行實(shí)現(xiàn)，具體如圖9 所示，輸入量為下一周期期望角度及當(dāng)前周期機(jī)械臂實(shí)際運(yùn)動角度，輸出為各關(guān)節(jié)所需驅(qū)動力矩，驅(qū)動機(jī)械臂到達(dá)期望位置；藍(lán)色部分為系統(tǒng)的導(dǎo)納控制部分，q1_r,q2_r,q3_r,q4_r 為使用者的期望角度。具體到adimittance模塊內(nèi)部，由4 個各關(guān)節(jié)導(dǎo)納控制組成，以關(guān)節(jié)1 為例（圖10）。q1_r 為期望角度，q1_v 為當(dāng)前位置，兩者差值乘以環(huán)境剛度ke，結(jié)合式（9）可以得到使用者的仿真輸入力矩TOL1，作為導(dǎo)納模型的輸入，在variable_admittance 模塊中，利用式（7），改變當(dāng)前周期的阻尼參數(shù)b1，最終得到機(jī)械臂下一周期所需到達(dá)的位置q1_d。

圖8 系統(tǒng)仿真Simulink 圖

圖9 位置環(huán)VDC 控制

圖10 改進(jìn)變導(dǎo)納控制器

仿真實(shí)驗(yàn)期望動作軌跡采用笛卡爾空間下均勻畫圓，利用機(jī)械臂逆向運(yùn)動學(xué)解出q1_r,q2_r,q3_r,q4_r 作為所需軌跡輸入，結(jié)合Adams 與MATLAB/Simulink進(jìn)行聯(lián)合仿真。仿真實(shí)驗(yàn)采用的主要參數(shù)為：

3.2 實(shí)驗(yàn)結(jié)果與分析

實(shí)驗(yàn)結(jié)果如圖11～13 所示。圖11 為3 種方法下的機(jī)械臂示教跟隨效果。在起始時有些許超調(diào)，之后康復(fù)機(jī)械臂跟隨性較好。

圖11 仿真實(shí)驗(yàn)機(jī)械臂末端位置

圖12 為具體的機(jī)械臂末端位置在x、y、z這3個方向上的誤差，由于是在xz平面內(nèi)畫圓，故而y方向誤差較小。誤差數(shù)據(jù)整理如表2 所示。在最大誤差絕對值方面，除起始時，改進(jìn)導(dǎo)納及改進(jìn)變導(dǎo)納控制器在x方向超調(diào)量較大外，后續(xù)過程中，兩種方法均可降低常規(guī)導(dǎo)納控制的跟隨誤差；在平均絕對值誤差方面，改進(jìn)導(dǎo)納控制可以將常規(guī)導(dǎo)納的誤差降低到近一半，而改進(jìn)變導(dǎo)納控制可以進(jìn)一步降低機(jī)械臂的跟隨誤差，將機(jī)械臂末端位置跟隨精度控制在2 mm 以下，提高示教過程精度。

圖12 仿真實(shí)驗(yàn)機(jī)械臂末端位置誤差

表2 仿真實(shí)驗(yàn)誤差分析 mm

圖13 反映的是3 種控制方法下，各關(guān)節(jié)人機(jī)交互力矩的大小，整理分析數(shù)據(jù)見表3。在力矩峰值方面，改進(jìn)變導(dǎo)納控制可以很好地降低力矩峰值，與常規(guī)導(dǎo)納相比，其關(guān)節(jié)1 交互力矩峰值降低45.30%，關(guān)節(jié)2 交互力矩峰值降低33.07%，關(guān)節(jié)3 交互力矩峰值降低10.99%，關(guān)節(jié)4 交互力矩峰值降低19.67%，大大減少了示教過程中使用者的負(fù)擔(dān)，提高了人機(jī)交互的舒適性；在人機(jī)交互力矩平均值方面，改進(jìn)變導(dǎo)納的各關(guān)節(jié)平均交互力矩不到常規(guī)導(dǎo)納的一半，也進(jìn)一步說明，在整個示教過程中，改進(jìn)變導(dǎo)納方法更加輕便。

圖13 仿真實(shí)驗(yàn)各關(guān)節(jié)人機(jī)交互力矩曲線

表3 仿真實(shí)驗(yàn)人機(jī)交互力矩分析 N·m

4 實(shí)物試驗(yàn)及結(jié)果分析

4.1 試驗(yàn)裝置及過程設(shè)計

為進(jìn)一步驗(yàn)證上述理論的有效性，對康復(fù)機(jī)械臂系統(tǒng)進(jìn)行實(shí)物畫圓示教試驗(yàn)。利用實(shí)驗(yàn)室開發(fā)的康復(fù)機(jī)械臂，在手臂末端固接一只馬克筆，白板上固定一個直徑180 mm的參考圓（圖14）。試驗(yàn)過程如圖15 所示，實(shí)驗(yàn)者通過穿戴康復(fù)機(jī)械臂，利用末端馬克筆在白板上參考畫圓，每次重復(fù)畫圓10 次。結(jié)合圖2，試驗(yàn)時，穿戴者作用于機(jī)械臂，通過拉壓力傳感器采集數(shù)據(jù)，乘以輸出桿的長度，再減去機(jī)械臂關(guān)節(jié)輸出扭矩及各關(guān)節(jié)重力補(bǔ)償力矩，得到人機(jī)交互力矩，作為導(dǎo)納控制系統(tǒng)的輸入。實(shí)驗(yàn)過程中，下位機(jī)通過串口上報上位機(jī)各關(guān)節(jié)角度及計算得到的人機(jī)交互力矩，上報周期10 ms。試驗(yàn)參數(shù)除Kd=0.3外，其余同3.1 節(jié)參數(shù)設(shè)置。采用常規(guī)導(dǎo)納控制與改進(jìn)變導(dǎo)納兩種方法進(jìn)行穿戴康復(fù)示教試驗(yàn)。

圖14 示教試驗(yàn)平臺

圖15 示教試驗(yàn)過程

4.2 實(shí)驗(yàn)結(jié)果及分析

利用機(jī)械臂正向運(yùn)動學(xué)，將采集到的各關(guān)節(jié)運(yùn)動角度轉(zhuǎn)換為機(jī)械臂末端馬克筆的空間位置。記錄整理數(shù)據(jù)如圖16～18 所示。

圖16 常規(guī)導(dǎo)納控制法示教實(shí)物試驗(yàn)畫圓

圖16和圖17 分別是采用常規(guī)導(dǎo)納控制方法和本文提出的改進(jìn)變導(dǎo)納控制方法穿戴示教畫圓得到的結(jié)果。常規(guī)導(dǎo)納方法的多次畫圓之間重復(fù)性較低，且偏離參考圓較多。這一點(diǎn)也體現(xiàn)在表4 中。采用改進(jìn)變導(dǎo)納控制方法，降低了跟隨誤差的最大值，且與常規(guī)導(dǎo)納控制方法相比，標(biāo)準(zhǔn)差降低11.46%，平均誤差絕對值降低25.09%。這說明采用改進(jìn)變導(dǎo)納控制方法，使用者示教過程更加平穩(wěn)，重復(fù)性較好，平均誤差也大大降低。

圖17 改進(jìn)變導(dǎo)納控制法示教實(shí)物試驗(yàn)畫圓

表4 實(shí)物試驗(yàn)誤差分析 mm

各關(guān)節(jié)的人機(jī)交互力矩如圖18 所示，改進(jìn)變導(dǎo)納控制方法在整體上比常規(guī)導(dǎo)納的力矩要小。分析人機(jī)交互力矩絕對值的特征信息匯總于表5。除關(guān)節(jié)1 兩種方法的區(qū)別不大外，在關(guān)節(jié)2～4 方面，改進(jìn)變導(dǎo)納控制方法均在不同程度上降低了人機(jī)交互力矩絕對值的最大值、標(biāo)準(zhǔn)差及平均值，其中最大值方面以關(guān)節(jié)4 方面降的最多，降低了63.44%；標(biāo)準(zhǔn)差及平均值以關(guān)節(jié)2 方面降低最多，分別為23.27%及20.79%。結(jié)合輸出連桿長度為65 mm，示教過程中，使用者作用于各關(guān)節(jié)的力不超過2.23 N，示教負(fù)擔(dān)較小。

圖18 實(shí)物試驗(yàn)人機(jī)交互力矩曲線

表5 實(shí)物試驗(yàn)人機(jī)交互力矩絕對值分析 N·m

盡管新提出的導(dǎo)納控制器降低了常規(guī)導(dǎo)納控制的跟隨誤差，但仍然有平均2.462 6 mm的誤差。這主要有以下兩個原因：

（1）機(jī)械臂桿長的放大作用。串聯(lián)機(jī)械臂各旋轉(zhuǎn)關(guān)節(jié)均安裝有諧波減速器，系統(tǒng)具有一定程度的柔性，剛度較低，會導(dǎo)致位置內(nèi)環(huán)的誤差。本文采用的機(jī)械臂僅小臂桿長就有470 mm，如果關(guān)節(jié)位置誤差精度控制在0.005 rad，轉(zhuǎn)換到末端空間位置誤差就有近2.35 mm的誤差。

（2）本文采用的導(dǎo)納控制器設(shè)計是基于關(guān)節(jié)空間的，但畫圓時，使用者通過目光觀察，接受到的畫圓誤差反饋是在笛卡爾空間下，其做力矩調(diào)整也是在x、y、z這3 個方向，而不是直接調(diào)整所需的關(guān)節(jié)角度，故而也會一定程度上增加系統(tǒng)的跟隨誤差。

但總體來說，本文提出的改進(jìn)變導(dǎo)納控制器，可以將示教試驗(yàn)畫圓平均誤差控制在2.5 mm 以下，能夠滿足康復(fù)示教訓(xùn)練的需求，便于康復(fù)機(jī)械臂后續(xù)功能的開發(fā)。

5 結(jié)語

（1）針對康復(fù)機(jī)器人示教過程中跟隨效果不佳的問題，提出了關(guān)節(jié)空間下結(jié)合力矩微分項(xiàng)的導(dǎo)納控制方法，并創(chuàng)新性地采用雙曲正切函數(shù)實(shí)現(xiàn)變導(dǎo)納控制器的設(shè)計，提高機(jī)械臂的柔順性。

（2）利用康復(fù)機(jī)械臂的SolidWorks 模型，結(jié)合Adams 及MATLAB/Simulink 搭建了仿真實(shí)驗(yàn)平臺，并進(jìn)行畫圓示教實(shí)驗(yàn)。仿真結(jié)果表明，提出的改進(jìn)變導(dǎo)納控制器可以降低示教跟隨誤差，實(shí)現(xiàn)較高精度的示教過程，且人機(jī)交互力矩也較小。

（3）畫圓康復(fù)示教實(shí)物試驗(yàn)結(jié)果表明，本文提出的結(jié)合力矩微分項(xiàng)的雙曲正切函數(shù)變導(dǎo)納控制器能夠很好地跟蹤示教者的動作，重復(fù)性好，降低了人機(jī)交互力矩，提高了系統(tǒng)的柔順性，改善了機(jī)械臂隨動效果。