基于惯性传感器遥控示教康复系统的研究

胡旭辉 王钰

摘要：针对上肢康复机器人的被动康复训练，建立了一套基于惯性传感器且用于UR协作机器人的实时路径规划的示教系统。利用固定于示教者手心处的惯性传感器，实时获取手部相对于大地坐标系的位姿数据，经过坐标变化，转变为机器人末端在机器人机架坐标系中的位姿数据，再由通讯系统传给机器人系统，最后经逆向运动学转变为机器人各关节坐标数据，进而实现机器人带动患者上肢进行康复运动。现场测试验证了该系统功能的有效性。

关键词：康复机器人;惯性传感器;被动运动;坐标变换

中图分类号：TP24         文献标志码：A

目前，中国脑卒中的发病率逐年增长，且趋于年轻化，预计到2030年将比2010年升高近50%，其中70%～85%的卒中后患者会伴有偏瘫后遗症[1]，即半侧肢体的运动失能。肢体失能的程度分为三级，一级是最严重的情况[2]，此时患者的运动康复需采用被动运动康复模式，即患者的上肢或下肢需在外力的帮助下进行长时间的特定运动。显然，辅助患者运动的外力若靠人工来实施，劳动强度非常大，且难以长时间保持规定的运动动作而导致康复效果欠佳，因而机器人辅助患者实施被动运动康复就成为康复领域的有效手段[3-7]。机器人辅助患者做被动运动康复，无论采用何种结构，都需要先规划机器人的运动轨迹[8-10]。而传统示教方式是以屏控设置轨迹坐标点[11]和末端拖拽为主[12-13]，实践发现这种示教方法难于满足安全性、高效性和快速适应性的基本要求[14]。对此，本文给出了一种基于惯性传感器示教的方法。先将一只惯性传感器置于示教者左或右手的中心（绑缚于手背上），通过校准获得初始位姿。然后获取传感器随示教者手部运动生成于大地坐标系的方位数据，并实时变换到手部坐标系，再实时变换为机器人的TCP方位坐标，通过逆向运动学实时计算出机器人的关节坐标，以此驱动机器臂带动患者上肢做被动康复运动。实测验证了机器人末端TCP的运动方位跟随惯性传感器运动方位的实时有效性，并给出了实际示教运动过程的演示实例。

1 惯性示教系统的组成

为了易于普及使用，本文选用了市面上常见的肢体运动惯性捕捉设备，型号为VDSuit，属于成本和精度都适中的产品，由17只惯性传感器、1只通讯转换器、若干连线和绑带所组成，如图1所示。

该运动惯性捕捉系统可以完整捕捉上、下肢体关节的整体空间运动，可以选用1只固定于手背中心，专门捕捉左或右上肢手部中心的轨迹数据，也可同时将两只传感器各置于左、右手中心，同时捕捉两条上肢末端的轨迹方位数据。当然，也可以全穿戴同时捕捉上下肢体各关节的运动数据，为未来实现上、下肢协同运动康复提供全面的运动数据。本文的研究只限于1只固定于左手或右手中心的惯性传感器，运动数据将变换给UR10机器人，最终驱动机器人的TCP沿惯性传感器的运动轨迹来运动，以此实现左上肢或右上肢的被动运动康复。

2 示教控制算法的实现

如图2所示，为该康复系统的初始狀态，其中站立者为示教康复师。有4组坐标描述该系统的空间状态：惯性系统的大地坐标Ce、位于示教者手部的传感器坐标Ch、绑缚患者手部的机器人末端TCP坐标Ct和机器人机架坐标Cb。传感器的运动数据首先被描述在Ce中，然后经坐标变换链Ce→Ch→Ct→Cb被映射为机架坐标数据，总变换矩阵

EAB  = EAH×HAt×tAB （1）

最后由上位机进行逆向运动学计算出机器臂的关节坐标数据，并经由通讯口发送给机器人控制系统，进而实现机器人的运动控制。

2.1 示教控制流程设计

基于惯性传感器控制康复机器人的整体算法流程如图3所示。手部传感器获取操作者手部运动数据，在惯性捕捉系统下生成相对于大地坐标系的运动数据，将这些运动数据通过蓝牙通讯传送给上位机，在上位机上实现一系列坐标变换及逆向运动学计算，且时刻判断示教动作是否超出安全区域或安全速度，若是，则生成康复机器人安全制动指令，通过Socket通讯发送给康复机器人，以保护患者免受二次伤害;反之，生成康复机器人运动控制指令，使得康复机器人跟随操作者做被动康复运动。

2.2 大地坐标系转换为手部初始坐标系

如图4所示，虚线表示操作者手部任意运动轨迹，点o为操作者手部初始位置，即手部坐标系Ch的位置，点i为运动过程中的任意点，通过固定在操作者手背中部的惯性传感器，可以获取任意时刻操作者手部相对于大地坐标系Ce的方位数据。其中，手部坐标系Ch的位置Pe=（px，py，pz），其对应的平移变化矩阵为

ETH  = Trans（px ，py ，pz ）（2）

以及欧拉角Ee=（φe，θe，ψe），本文姿态变换采用欧拉角中ZYX型，其对应的旋转矩阵为

ERH  = RPY（ψe ，θe ，φe ）（3）

由式（2）、（3）可得到操作者手部坐标系Ch在大地坐标系下的位姿矩阵为

EAH  = ETH ×ERH  = cos ψe cos θe  cos ψe sin θe sin φe -sin ψe cos φe  cos ψe sin θe cos φe  + sin ψe sin φe  px sin ψe cos θe  sin ψe sin θe sin φe  + cos ψe cos φe  sin ψe sin θe cos φe -cos ψe sin φe  py -sin θe  cos θe sin φe  cos θe cos φe  pz 0 0 0 1（4）

同理，得到在大地坐标系Ce下操作者手部任意点i的位姿EAi。设手部任意点i在手部初始坐标系Ch中的位姿为HAi

EAi  = EAH ×HAi （5）

则手部任意点i在手部初始坐标系Ch下的位姿矩阵为

HAi  = （EAH ）-1×EAi （6）

因此可得到手部任意点i在手部初始坐标系Cn下的位姿为

HAi  = （hpxi ，hpyi ，hpzi ，φhi，θhi ，ψhi ）（7）

2.3 手部坐标系转换为工具末端坐标系

由于要使机器人TCP与带有惯性传感器的手部进行同步运动，以及位置与姿态运动变化同步，即要使机器人末端在TCP初始坐标系Ct中的运动状态完全与手部在手部初始坐标系Ch中的运动状态相同，于是可得到机器人末端在坐标系Ct下随手部运动的位姿为Ptcp=（xtcp，ytcp，ztcp，φtcp，θtcp，ψtcp）。其中

xtcp ytcpztcpφtcpθtcpψtcp = 0 -1 0 0 0 0-1 0 0 0 0 00 0 -1 0 0 00 0 0 0 -1 00 0 0 -1 0 00 0 0 0 0 -1×hpxi hpyi hpzi φhi θhi ψhi （8）

2.4 TCP坐标系转换为机器人机架坐标系

本文所选用的机器人为UR10，建立其初始关节坐标系如图5所示，x0y0z0为机器人机架坐标系，x6 y6 z6为机器人末端TCP坐标系。

根据链式机器人臂的运动学理论，得到机器人机架坐标系到机器人末端TCP坐标系的齐次变换矩阵

BTtcp=A1 A2 A3 A4 A5 A6 （9）

其中

Ai+1=cos θi+1-sin θi+1cos αi+1sin θi+1sin αi+1ai+1cos θi+1sin θi+1cos θi+1cos αi+1-cos θi+1sin αi+1ai+1sin θi+10sin αi+1cos αi+1di+10001

可得在机架坐标系下，機器人TCP随操作者手部运动的位姿，通过控制器，求出该位姿所对应的各关节旋转角，使得康复机器人跟随着示教者做相应的康复运动。

3 示教运动过程测试

3.1 跟随精度与重复精度

在坐标变换系统，操作者带动康复机器人做一组连续运动，以检测：（1）跟随精度，即机器人末端TCP能否准确地跟随操作者手部进行运动;（2）重复精度，即在运行一段时间后，操作者回到初始方位时，康复机器人TCP能否回到所对应的初始方位。具体测量结果如图6（a）～（f）所示，其中实线表示手部传感器在各方向的位置与姿态，虚线表示康复机器人TCP在各方向的位置与姿态，水平直线表示手部传感器与康复机器人的初始位姿。

（a）手部传感器沿x方向及所对应TCP的位置;（b）手部传感器绕x方向及TCP所对应的姿态;（c）手部传感器沿y方向及所对应TCP的位置;（d）手部传感器绕y方向及TCP所对应的姿态;（e） 手部传感器沿z方向及所对应TCP的位置;（f） 手部传感器绕z方向及TCP所对应的姿态在图6（a）～（f）中，实线与虚线基本重合，各方向的最大位置跟随精度为±4 mm，最大姿态跟随精度为-0.4°～0.6°，说明机器人末端TCP能够准确地跟随操作者手部进行康复运动，因此本文所建立的整套坐标变换是正确的。对于重复精度，在运动一段时间之后，操作者手部回到初始位置，机器人末端TCP在x、y、z方向的位置与其初始位置的误差分别为19.54 mm、30.87 mm、8.55 mm，各方向的姿态误差分别为-0.18°、0.278°、-1.13°，因为手部传感器数据漂移与误差累积的缘故。因此，可通过误差补偿的方法减小各方向的位置误差，且示教过程最好在5 min内完成，不易过长。

由于本文所选用的是在能够满足精度的前提下，成本较低的惯性传感器，其在测量时会产生数据漂移，造成数据不准确，且随测量时间的变长，也会产生误差积累。

3.2 实际康复动作测试

为了更加直观，操作者带动患者做一组倒水的康复运动，如图7（a）～（i）所示，操作者带动患者从初始位置出发，去抓取装有水的杯子，带着患者将水倒入另外一个空杯中，倒完水后，将杯子放回原来的位置，最后，操作者带着患者回到初始位置。

4 安全保护功能的设计

安全性是康复机器人需要考虑的重要因素之一。示教过程中，示教者穿戴惯性传感器的手有可能意外失速或超出患者的安全运动区域，造成患者的二次伤害，故该系统采用了速度安全保护与限位空间保护的控制措施。

4.1 速度安全保护

速度安全保护的关键点在于如何给定安全速度值。本文根据调研与实际测试可知，当机器人TCP的加速度不超过重力加速度的30%时，手部绑缚于机器人TCP处的患者在感觉上是可接受的，此时若TCP的速度为零，则1 s后其速度将接近300 mm/s，故为了简化判断过程，认为TCP在任意时刻的速度达到300 mm/s，控制系统就应制动。进一步测试发现，尽管TCP的加速度很小，但让其长时间保持一个较高速度运动，则该速度不宜超过275 mm/s为合适。本文选定最大安全速度为275 mm/s，如图8所示。控制系统实时监测康复机器人末端x、y和z方向的合成速度，若合成速度达到最大安全速度时，即可对康复机器人发出停止运动指令，以达到保护患者的作用。

4.2 限位空间保护

对于限位空间保护，以患者的肩关节为中心，患者的手伸向远端时，不得超过上肢伸直的长度，建立约束平面Ⅰ;患者的手向近端移动时，不得触及自己的躯干，建立约束平面Ⅱ;上肢向下屈的范围，则不能越过桌面或触及患者自身的下肢，建立约束平面Ⅲ，而向上展的范围，则根据患者的具体情况设定，但不能超过180°，建立约束平面Ⅳ。如图9（a）、（b）所示，以肩关节为原点，肩部方向为x轴，手臂前伸方向为y轴，垂直向上为z轴，建立空间坐标系，则约束平面Ⅰ的参数方程为

x=Lcos αy=Lsin αz=t （10）

其中，α∈[π6，5π6]，t∈（-SymboleB@，+SymboleB@），L表示臂长，α表示手臂绕Z轴旋转角度。

约束平面II的方程为

A2x+B2y+C2z-d2=0A2=C2=0B2=1 （11）

其中，d2表示近端安全距离。

约束平面Ⅲ、Ⅳ的方程为

A3x+B3y+C3z+d3=0A4x+B4y+C4z-d4=0A3=B3=A4=B4=0C3=C4=1 （12）

其中，d3、d4表示手臂下屈与上展的安全高度。

限位空间的实际尺寸，可根据不同患者的实际肢体尺寸具体设定，控制系统实时监测机器人末端的位置，若TCP超出限位空间，即刻对康复机器人发出停止运动指令。

5 结论

本文基于惯性传感器建立了一套用于被动运动康复的示教系统，该系统对安全性的实现给予了充分的重视，通过对速度安全与限位空间安全的实现，避免了因示教者的意外失误对患者可能造成的二次伤害。同时，该系统在一定的时间范围内，能够更加安全、更加有效地进行康复示教，解决了传统康复机器人示教难的问题。后续研究将通过误差补偿的方法进一步减小传感器的累积误差，使得该系统能够更加精确、更加持久地对康复机器人进行示教。

参考文献

[1]王陇德，刘建民，杨弋，等.我国脑卒中预防仍面临巨大挑战——《中国脑卒中防治报告2018》概要[J].中国循环杂志，2019，34（2）：105-119.

[2]张晶，王斌全，魏邵辉.脑卒中失能老年患者辅助器具使用情况的调查分析[J].护理学杂志，2017，32（1）：85-88.

[3]杨启志，曹电锋，赵金海.上肢康复机器人研究现状的分析[J].机器人，2013，35（5）：630-640.

[4]KWAKKEL G， WAGENAAR R C， TWISK J W R， et al. Intensity of leg and arm training after primary middle-cerebral-artery stroke： A randomized trial[J].The Lancet，1999，354（9174）：191-196.

[5]王娟.上肢康復机器人训练对脑卒中后偏瘫患者上肢功能的影响[J].按摩与康复医学，2017，8（7）：21-22.

[6]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.

[7]孙超，苑明海，周灼，等. 外骨骼上肢康复机器人的结构设计与仿真研究[J]. 机电工程，2019，36（4）：383-386.

[8]KIM H， MILLER L M， FEDULOW I， et al. Kinematic data analysis for post-strokepatients following bilateral versus unilateral rehabilitation with an upper limb wearable robotic system[J]. IEEE Transactions on Neural Systems and Rehabilitation Engineering， 2013， 21（2）：153-164.

[9]TANG L W， TANG X Q， JIANG X L， et al. Dynamic trajectory planning study of planar two-dof redundantly actuated cable-suspended parallel robots[J]. Mechatronics， 2015， 30： 187-197.

[10] ABU-DAKKA F J， RUBIO F， VALERO F， et al. Evolutionary indirect approach to solving trajectory planning problem for industrial robots operating in workspaces with obstacles[J]. European Journal of Mechanics A-Solids， 2013， 42： 210-218.

[11] 计时鸣，黄希欢.工业机器人技术的发展与应用综述[J].机电工程，2015，32（1）：1-12.

[12] 李占贤，李兴龙.一种新型手把手示教型施釉机器人[J].机械，2015，42（1）：67-69.

[13] 刘乃军，鲁涛，蔡莹皓，等.机器人操作技能学习方法综述[J].自动化学报，2019，45（3）：458-470.

[14] KAWASAKI H， NANMO S， MOURI T，et al. Virtual robot teaching for humanoid hand robot using muti-fingered haptic interface[C]// 2011 International Conference on Communications， Computing and Control Applications （CCCA），Tunisia，2011： 1-6.

Research on Remote Teaching and Rehabilitation System Based on Inertial Sensor

HU Xu-hui， WANG Yu

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

Abstract：

Aiming at the passive rehabilitation training of upper limb rehabilitation robots， a teaching system based on inertial sensors and used for real-time path planning of UR collaborative robots was established in this survey. Firstly， an inertial sensor fixed on the palm of the teacher was used to obtain the pose data of the hand relative to the earth coordinate system in real time. After a series of coordinate changes， it was transformed into the position and posture data of the robot′s tool in the coordinate system of the robot's frame. Then， it was transferred from the communication system to the robot system. Finally， it was transformed into the coordinate data of each joint of the robot through reverse kinematics. By testing， the rehabilitation system has shown an excellent result.

Keywords：

rehabilitation robot; inertial sensor; passive motion; coordinate transformation

收稿日期：2021-06-29

基金項目：

山东省科技发展计划项目（批准号：40214010075）资助。

通信作者：

王钰，男，副教授，主要研究的方向为康复机器人与打磨机器人。E-mail：ywang@qdu.edu.cn

2843501186293