人机末端交互运动中轨迹跟随控制算法

刘环宇 陈明祥 高孟扬 田启磊 王钰

摘要：

为了解决上肢康复机器人的灵活性低、体积笨重和康复效果差等问题，提出了使用协作机器人UR10作为一种新的康复辅助工具的控制算法。通过建立UR10的运动学模型和患者手臂运动学模型，确定患者手臂与UR10协作机器人末端交互运动的灵巧空间，获得机器人与患者合理的相对安放位置。在患者上肢手掌心能够到达的运动空间中，选定两条距离长度最远的空间轨迹，利用Robotics Toolbox对机器人末端沿代表性轨迹进行仿真，验证轨迹跟随控制的准确性。现场测试也验证了轨迹跟随和灵巧空间的有效性。

关键词：

康复机器人；运动学分析；轨迹跟随

中图分类号：

TP24

文獻标志码：A

上肢的主动康复运动是通过上肢康复设备追随患者的手，努力保持沿给定轨迹的一种运动［1-2］。中国40岁及以上人群上肢偏瘫超1 200万人，传统的人工康复医疗师已经无法满足康复医疗市场的需求，而且人工康复成本高、任务重、周期长［3-6］ ，耽误脑卒中患者康复的最佳时间，造成永久性的偏瘫。因此，引入机器人辅助患者完成一定的康复训练成为目前解决问题的有效手段［7-10］。现有的一款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函数只选择其中一个最优解。现实情况下，要综合考虑所有要求，选择一个最适合的解。为研究运动状态下的平稳性，随机选取任意状态下的轨迹，图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