人機末端交互運動中軌跡跟隨控制算法

劉環宇 陳明祥 高孟揚 田啟磊 王鈺

摘要：

為了解決上肢康復機器人的靈活性低、體積笨重和康復效果差等問題，提出了使用協作機器人UR10作為一種新的康復輔助工具的控制算法。通過建立UR10的運動學模型和患者手臂運動學模型，確定患者手臂與UR10協作機器人末端交互運動的靈巧空間，獲得機器人與患者合理的相對安放位置。在患者上肢手掌心能夠到達的運動空間中，選定兩條距離長度最遠的空間軌跡，利用Robotics Toolbox對機器人末端沿代表性軌跡進行仿真，驗證軌跡跟隨控制的準確性?，F場測試也驗證了軌跡跟隨和靈巧空間的有效性。

關鍵詞：

康復機器人；運動學分析；軌跡跟隨

中圖分類號：

TP24

文獻標志碼：A

上肢的主動康復運動是通過上肢康復設備追隨患者的手，努力保持沿給定軌跡的一種運動［1-2］。中國40歲及以上人群上肢偏癱超1 200萬人，傳統的人工康復醫療師已經無法滿足康復醫療市場的需求，而且人工康復成本高、任務重、周期長［3-6］ ，耽誤腦卒中患者康復的最佳時間，造成永久性的偏癱。因此，引入機器人輔助患者完成一定的康復訓練成為目前解決問題的有效手段［7-10］?，F有的一款4自由度康復機器人通過綁帶固定患者的大臂和小臂，實現肩關節與肘關節的運動跟隨運動，然而只能進行一定范圍的人機交互康復運動且軌跡追蹤效果誤差較大［11］ 。另一款7自由度的康復機器人可以實現空間范圍內的康復運動，軌跡追蹤也達到了預期效果，但沒有對碰撞安全性的問題進行說明［12］ 。6自由度康復機器人將電機直接安裝于上肢各關節處，導致機械臂龐大、笨重，致使軌跡跟隨的效果與理想效果差距較大，人機交互能力差［13］ 。本文通過協作機器人UR10的末端與患者手掌心末端始終跟隨一起運動的交互方式，確定靈巧空間，并驗證UR10機器人的末端與患者手掌心在交靈巧空間中，患者手掌心的軌跡能夠被UR10順暢的追隨。

1 人機交互模型的建立

為了改善和優化人與機器之間的交流和互動，建立人機交互模型如圖1所示。模型中患者的患肢手掌心與協作機器人末端的TCP在整個交互過程中始終保持重合，則掌心的位姿即為TCP追隨的位姿，或掌心的運動軌跡即為TCP追隨的軌跡。設手掌心坐標為Ch，TCP工具坐標為Ct，機器人末端坐標為Ce，機器人機架坐標為Cb，則Ct=Ch，機器人控制系統追隨患者掌心位姿的總變換矩陣為

Abt=AbeAet（1）

其中，Abt是總變換矩陣，Aet是掌心到機器人端面的變換矩陣，Abe是機器人端面到機架的變換矩陣，由UR10協作機器人的D-H參數計算。

機器人追隨手掌心的位姿

nbobabpbnbobabpbnbobabpb0001T=Abtntotatptntotatptntotatpt0001T（2）

在計算與仿真中，根據中國成年人人體尺寸（GB10000-88）［14］ ，患者模型身高取170 cm。

1.1 患者手掌心運動空間的確定

為了保證患者在康復運動中的安全性，建立一個基于患者人體安全位置的OBB運動范圍包圍盒（Oriented Bounding Box），即包含某一對象最小的長方體。

OBB運動范圍包圍盒采用蒙特卡羅方法仿真驗證，即在規定的關節角度范圍內，使用隨機組合的方式生成各個關節的角度，然后利用機器人正運動學模型計算機器人末端執行器的位置。為了保證仿真的準確性，使用rand（sz1，…，szN）函數生成一個在空間范圍內均勻分布的隨機數數組，其中，sz1，…，szN表示每個維度的大小。這樣確保得到的隨機數在整個空間范圍內具有均勻分布的特點，從而避免了由于隨機數分布不均勻而導致的仿真誤差。表1中是基于OBB運動包圍盒設置的活動關節角度范圍。

OBB運動包圍盒近人體的面與患者身體保持100 mm的距離，底面與患者的腿平面保持100 mm的距離，寬度等于患者小臂的長度，頂面的距離為患者大臂加上小臂的長度，相對位置如圖2所示。

在Matlab下建立患者手臂的運動學模型（見2.1節）并仿真出患者手掌心的運動范圍（圖3），運動范圍包括患者在活動期間可能出現的所有位置點，通過將運動范圍內所有的位置點與OBB進行比較，驗證了該OBB包圍盒的正確性，OBB的合理性驗證是為了確保該包圍盒能夠完整地包含所有可能的運動軌跡，同時保障患者在運動過程中的安全性。

1.2 UR10機器人運動空間的確定

基于確定患者手掌心運動空間范圍的實驗方法，進而確定UR10在仿真環境下的運動范圍。表2為UR10關節的活動范圍，由于UR10機器人的位置主要是由前3個關節確定的，因此取其前3個關節。

UR10機器人的可達空間范圍近似圓形（圖4），與UR官方給予的工作空間相吻合，證明了運動范圍的正確性。

1.3 人機交互空間的確定

建立人機交互空間能夠實現患者與UR10機器人之間的協同運動，交互空間的準確性對于康復運動的成功實施具有直接的關聯與影響。為了構建有效的交互空間，采用了區間集合重疊區域的原理，通過調整患者手臂基坐標系相對于UR10機器人基坐標系的相對位置，得到一個集合重疊區域。在該重疊區域內，UR10機器人的可達工作空間完全覆蓋了患者手掌心的運動范圍，機器人的運動范圍包含了患者手掌心的運動范圍，此時患者基坐標系應距離機器人基坐標系500 mm處，如圖5所示，圖中紫色六面體代表患者在日常生活中運動最頻繁的空間范圍［15］，也可稱其為靈巧空間［16］。

2 協作機器人UR10的運動學驗證

UR10機器人有6個自由度，具備高度的靈活性和柔順性，可方便地進行編程和操作，價格適中，易于在社會范圍內推廣。三維建模后得到了機器人的運動學模型，通過對機器人的正逆運動學分析，可以實現機器人在運動過程中的精確控制和精準定位。

2.1 UR10與患者手臂的運動學模型

UR10建模時采用經典的D-H坐標變換法。相關參數包括關節長度、關節角度和DH參數等（表3），每一個關節都是旋轉關節，其中，θ是關節變量，a是相鄰關節轉軸的距離，d是相鄰關節中心的偏距，α是相鄰關節轉軸的夾角。

同理，根據中國成年人人體尺寸（GB10000-88），可以建立患者手臂的運動學模型，與UR10模型放在同一三維工作空間中，如圖6所示。

2.2 UR10末端的軌跡規劃與控制

本文采用ikine（T，'mask'，mask）函數驗證UR10逆運動學的關節角度，其中，T代表機器人末端的位姿矩陣，mask代表機器人的自由度。

末端控制算法的核心思想是將患者手掌心位姿變換為UR機器人的運動位姿，然后利用逆運動學求解出UR的6個關節角度的關系變化值，將所求得的關節角度值賦給UR10機器人，使其能夠按照手掌心位姿運動軌跡進行協同運動。某一位姿下初始機器人6個關節角度（θ1=π/2，θ2=π/3，θ3=π/2，θ4=π/3，θ5=π/2，θ6=π/3），通過ikine求得上述相同位姿下的UR關節角度（弧度）為：（θUR1，θUR2，θUR3，θUR4，θUR5，θUR6）=（1.0472，1.1779，1.5746，-1.7053，-1.0472，-1.5708）。

在Matlab中求出的關節角度與設置的初始關節角度不完全相同，由于機器人的逆運動計算過程中存在多重解，而ikine函數只選擇其中一個最優解?，F實情況下，要綜合考慮所有要求，選擇一個最適合的解。為研究運動狀態下的平穩性，隨機選取任意狀態下的軌跡，圖7（a）給出了UR10運動的仿真實時軌跡，圖7（b）展示了6個關節在運動過程中角度、角速度、角加速度的變化過程。在UR10機器人的運動過程中，機器人的起點和終點關節角度均為0，由速度和加速度的曲線可知，機器人在仿真環境中運動較為平順，無抖動現象。

3 人機交互運動仿真

康復過程中需要機器人準確跟蹤患者手掌心的運動軌跡，以實現最佳的康復效果，采用關節空間軌跡的方法來驗證機器人是否能夠準確地追蹤患者手掌心的運動軌跡。

3.1 患者手掌心運動軌跡的確定

在規劃機械臂的運動軌跡時，采用Robotics Toolbox提供的關節空間軌跡規劃方法［17］，核心函數是jtraj（q0，q1，tv），該函數可以根據初始關節角度值q0、最終關節角度值q1和時間tv生成一條軌跡曲線。該函數最終的返回值為一個n×m的關節角度矩陣，其中n為機械臂的自由度數，m為設置的時間點數，通過正運動學算法，可以將關節角度轉變成患者手掌心的位姿，得到手掌心的實時位姿。因在靈巧空間中存在著無數條軌跡曲線，為了更好的證明軌跡曲線的有效性，選取空間中最遠的兩條代表性軌跡（最上與最下、最左與最右）。

3.2 UR10機器人末端追蹤軌跡的仿真

UR10能否有效地追蹤患者的運動軌跡是保障患者康復運動情況的重要衡量指標。為了驗證軌跡仿真的通用性和準確性，UR10將追蹤3.1節中描述的兩條代表性軌跡曲線。如果由極限點組成的這兩條軌跡可以被成功追蹤和執行，那么其他正常軌跡也可以被進行運動控制。

軌跡追蹤算法的核心思想是將患者手掌心的運動映射到UR10機器人的運動，以實現患者與機器人的協同運動。算法首先通過式（1）和（2）計算出手掌心位姿在人體基座標系和UR10機器人基座標系下的坐標變換關系，把手掌心的位姿變換成機器人末端位姿，然后利用逆運動學求解的方法，求解出患者手掌心位姿與UR10的6個關節角度的關系變化值。最后，將所求得的關節角度值賦給UR10機器人，通過jtraj函數控制其軌跡運動，控制算法流程如圖8所示。

基于1.3節提供的靈巧空間信息，圖9（a）中，患者手掌心的運動軌跡與UR10的軌跡仿真完全一致，證明了在仿真的理想環境下，UR10機器人能夠成功追蹤患者手掌心的運動軌跡。同時，記錄了UR10機器人在運動過程中處于左、右、上、下位置的位姿，如圖9（b）所示，這些位姿在特殊點處的數值正常，證明了在整個運動過程中，UR10機器人的位姿變化穩定，從而驗證了在仿真環境下，UR10機器人在靈巧空間中能夠適應不同位姿以執行康復運動。

3.3 樣機測試

為驗證算法的正確性以及靈巧空間的準確性，采用了延長理論軌跡的方法進行實驗。在實測環境下，UR10機器人TCP末端與患者的手通過綁帶的形式進行固定，通過網線端口建立上位機與UR10機器人的實時連接。將在仿真環境中記錄的手掌心理論位姿作為輸入，然后傳輸到UR10機器人的控制器中，記錄UR10機器人TCP的實際位姿變化，效果如圖10所示。在靠近靈巧空間邊緣的情況下，UR機器人并不會按照延長軌跡繼續運動，說明UR機器人可以在預先定義的人機交互的靈巧空間內進行相應的康復運動，驗證了靈巧空間的準確性和有效性。

實驗中，患者分別移動水杯和高空取物，并記錄活動過程中的始、終位姿。圖11（a）與（b）患者將水杯從最左側移動到最右側，圖11（c）與（d）患者將水杯從最高處取物并且放到最低處。圖中的Rx、Ry、Rz代表機器人的RPY角，運動過程中始端與末端的位姿不相同，均在變化。

圖12記錄了患者在進行兩種康復活動時關節角度的變化，在運動過程中，圖中6個關節角度的變化趨勢趨于穩定，無突變情況，證明機器人在運動過程中能夠保持平穩性和順滑性。實驗結果顯示，在靈巧空間中，患者的手掌心以及機器人TCP均可以到達極限點位置，UR10機器人的位姿與仿真結果位姿保持一致，運動過程中關節角度無突變情況，因此可以證明實測環境下患者的位姿是在有效、順滑的變化。

4 結論

本文提出了依靠協作機器人UR10來實現上肢康復運動的康復方法，通過 Matlab平臺規劃出人機交互的靈巧空間，并對其空間中的運動控制進行運動學的仿真分析，證明了軌跡的可靠性和有效性。UR10樣機的實測結果說明理論的可行性。后續研究將在運動控制算法上進行更系統、詳細的優化。

參考文獻

［1］梁文淵，畢勝.主動康復訓練機器人的感知交互與控制策略［J］.科技導報，2019，37（22）：26-36.

［2］楊曉斌，王鈺.一種小型等速康復裝置控制算法的仿真研究［J］.青島大學學報（自然科學版），2022，35（2）：78-82.

［3］胡旭輝，王鈺.基于慣性傳感器遙控示教康復系統的研究［J］.青島大學學報（自然科學版），2022，35（1）：66-72.

［4］KNEPLEY K D， MAO J Z， WIECZOREK P，et al. Impact of telerehabilitation for stroke-related deficits［J］. Telemedicine and e-Health， 2020， 27（3）： 239-246.

［5］HARRIS N R， STHAPIT D. Towards a personalised rehabilitation system for post stroke treatment［C］// 11th IEEE Sensors Applications Symposium （SAS）. Catania， 2016： 217-221.

［6］李思奇，趙依雙，張玉梅.人工智能在腦卒中后認知障礙評定與康復中的應用［J］.中國醫刊，2019，54（10）：1066-1070.

［7］楊啟志，曹電鋒，趙金海.上肢康復機器人研究現狀的分析［J］.機器人，2013，35（5）：630-640.

［8］王娟.上肢康復機器人訓練對腦卒中后偏癱患者上肢功能的影響［J］.按摩與康復醫學，2017，8（7）：21-22.

［9］RHON D I， GILL N， TEYHEN D， et al. Clinician perception of the impact of deployed physical therapists as physician extenders in a combat environment［J］. Military Medicine， 2010，175（5）： 305-312.

［10］ 田啟磊，王鈺.利用慣性傳感器數據實時示教下肢外骨骼步態的研究［J］.青島大學學報（自然科學版），2023，36（1）：54-59.

［11］ 張偉勝，喻洪流，黃小海，等.一種新型四自由度的上肢康復機器人［J］.中國康復理論與實踐， 2019， 25（10）： 1202-1208.

［12］ YANG Q Z， FANG J J， MA X P， et al. Kinematics and workspace analysis of 7-DOF upper-limbed rehabilitation robot［C］// IEEE International Conference on Cyber Technology in Automation， Control， and Intelligent Systems （CYBER）. Shenyang， 2015： 353-358.

［13］ 李雙雙. 六自由度上肢康復機器人機構設計與控制研究［D］.秦皇島：燕山大學， 2021.

［14］ 中國國家技術監督局，中國國家標準化管理委員會.中國成年人人體尺寸：GB/T 10000-1988［S］. 北京： 中國標準出版社， 1989.

［15］ NAMDARI S， YAGNIK G， EBAUGH D D， et al. Defining functional shoulder range of motion for activities of daily living［J］. Journal of Shoulder and Elbow Surgery，2012，21（9）： 1177-1183.

［16］ 趙智遠，徐振邦，何俊培，等.基于工作空間分析的9自由度超冗余串聯機械臂構型優化［J］.機械工程學報，2019，55（21）：51-63.

［17］ 國許東，王鈺，佟河亭.外骨骼式上肢康復機器人的軌跡規劃及仿真［J］.青島大學學報（自然科學版），2015，28（3）：65-69.

Trajectory Following Control Algorithm in the Interactive Motion of

the Uuman-machine End

LIU Huan-yu， CHEN Ming-xiang， GAO Meng-yang，

TIAN Qi-lei， WANG Yu

（College of Mechanical and Electrical Engineering， Qingdao University， Qingdao 266071， China）

Abstract：

In order to solve the problems of low flexibility， bulky size and poor rehabilitation effect of the upper limb rehabilitation robot， a control algorithm of using the collaborative robot UR10 as a new rehabilitation aid was proposed. The patient's arm kinematic model and the UR10 robotic arm's kinematic model were established to determine the dexterous workspace of interaction between the patient's arm and the UR10 collaborative robot's end-effector， allowing for the reasonable relative positioning of the robot and the patient. In the movement space that can be reached by the palm of the patients upper limbs， two spatial trajectories with the furthest distance were selected， and Robotics Toolbox was used to simulate the end of the robot along the representative trajectory to verify the accuracy of trajectory following control. Field tests also verified the effectiveness of track following

Keywords：

rehabilitation robot; kinematic analysis; track tracking

收稿日期：2023-03-19

基金項目：

山東省科技發展計劃項目（批準號：40214010075）資助。

通信作者：

王鈺，男，副教授，主要研究方向為康復機器人與打磨機器人。E-mail：ywang@qdu.edu.cn