随着我国经济的高速发展，上班及科研人群工作强度及工作时间大幅增加[1]。长时间处于久坐状态将影响健康状态，主要表现为肩颈、背部酸痛，不加以干涉则将进一步发展为肩周炎及颈椎病[2]。目前常用的改善方法是人工按摩，这需要经验老道的康复医师一对一的进行徒手或简单的康复训练。该方法虽然有一定效果，但仍存在效率低下、精确度无法保证等问题。按摩机器人以数字化手段复现康复医师的手法，并能达到更精确的力道及位姿控制，能够解决上述传统方法的缺点[3,4]。因此，开展按摩机器人相关研究非常必要。在国外，日本三洋电机的MINYONG P[5]等人于1997年通过PI伺服补偿及雅可比矩阵在四指机器类人手上复现了专家按摩师的按摩力。美国圣荷西州立大学的WANG Y C[6]等人于2013年通过设计并搭建双目视觉系统及CECT算法，实现了背部按摩关键点位的识别，日本丰桥技术科学大学的YASUMOTO Y等人于2020年开发了疲劳度与肌肉硬度相关研究，青山学院大学的HARADA N等人则于2023开发了一套按摩专家测量系统，这两项研究都将对未来按摩机器人复现专家级按摩提供了专家数据基础。在国内，山东建筑大学的鲁守银[7,8]等人于2011年基于设计的4自由度机械臂实现了10种典型的中医按摩手法。北京理工大学的LI Shuai[10]等人于2017年在PD控制的基础上增加了阻抗控制器，并分别在4自由度机械臂及双臂机器人上取得了良好的按摩效果。西北工业大学的LIU Y[11]等人于2019年设计并制作了一款带有移动底盘及按摩机构的小型背部智能按摩机器人，其通过四路红外传感器实现背部遍历，并实现了六种按摩手法，但是缺少按摩力的精准把控。北京航空航天大学的XING Kefan[12]等人设计了一款带有射频热疗模块、硅胶气囊、气囊加紧模块构成的一体化柔性按摩头，结合计算机视觉实现按摩区域的识别及路径规划，最后通过六轴协作机械臂实现按摩功能，但缺少按摩力的反馈闭环控制。浙江大学的DONG Huixu[13]等人利用Delta并联机构搭建并实现了三种按摩功能，在力位混合控制中做到了不大于1N的误差，但受限于并联机构的运动空间，该按摩机构仅适用于小范围肌肉组织按摩。上述研究大多关注按摩点位识别、规划及单个维度上按摩手法及力道的复现，而未考虑机械臂与患者皮肤接触过程中单维力位控制精度不够的问题。因此，本文利用六自由度协作机械臂及六维力传感器，设计了一套协作机器人按摩平台，结合重力补偿、导纳控制与虚拟力控制组成的闭环控制系统，实现按摩过程中机器人规划轨迹良好跟随情况下与皮肤的柔顺接触。本文首先介绍搭建的硬件平台及控制系统架构，然后对架构中的各个模块进行推导，最后通过实际采集的数据以验证所提出的控制方法的可行性与准确性。

1、系统架构

通过六轴的协作机械臂以及蓝点触控的六维传感器搭建了一套按摩机器人平台，如图1所示，并通过软橡胶及气囊模拟人体；算法部分通过实时重力补偿器、导纳控制器、虚拟力控制器以及运动执行器实现了康复按摩中精确的力位控制，算法流程图如图2所示。其中，实时重力补偿通过辨识末端负载在不同位姿下重力分力并予以消除，以提高机械臂的运动控制精度；导纳控制器基于建立的肌肉-机械臂末端力学模型，推导出末端位移增量输入，以实现柔顺的零力随动控制；虚拟力控制器通过引入虚拟夹具，基于当前位置与预期轨迹最小偏差矢量生成的虚拟力，来引导并修正机械臂的运动轨迹，以实现预期按摩轨迹的准确跟随；运动执行器接收运动控制指令，驱动机械臂运动。

图1按摩机器人硬件平台  

图2算法流程图  

2、算法

2.1末端实时重力补偿

本文搭建的按摩机器人末端带有六维力传感器以及按摩头，在机器人末端处于不同的姿态时，按摩头自身的重力会对六维力传感器产生一个作用力，导致六维传感器采集到的六维力数据耦合了外部接触力真值、按摩头重力分力以及传感器校准时的标定零力；同时，传感器及按摩头作为机械臂末端负载，其不同姿态下重力对手腕关节的作用会影响到末端控制的精度。因此，本文推导并实现了实时重力补偿算法，通过辨识六维力传感器标定值及当前重力分量，得到准确接触力真值，同时实现末端负载的实时重力补偿。具体算法推导如下。

给出按摩机器人末端受力分析如图3所示，图中P为假设的负载中心。在传感器坐标系Os下，给出传感器受力方程如式(1)所示：

图3机器人末端受力分析  

其中，Fx,Fy,Fz,Mx,My,Mz表示六维力传感器采集到的原始数据；fx,fy,fz,Mfx,Mfy,Mfz分别表示皮肤与按摩头的实际接触力对传感器施加的力以及力矩；Fx0,Fy0,Fz0,Mx0,My0,Mz0分别指传感器校准时自身的零力值；Gx,Gy,Gz,MGx,MGy,MGz分别表示按摩头重力在当前位姿下对传感器施加的力以及力矩。同时，在传感器坐标系下，给出负载中心P(x,y,z)重力在传感器处产生的力矩如式(2)所示：

当末端不处于工作状态，即按摩头未与皮肤接触时，有实际接触力fx,fy,fz,Mfx,Mfy,Mfz均为0，此时将式(2)代入式(1)中，可得式(3):

注意到负载中心坐标x,y,z以及传感器零力Fx0,Fy0,Fz0,,Mx0,My0,Mz0均为常数，定义常数项a1,a2,a3如式(4)，代入式(3)，并改写成矩阵形式可得式(5):

其中，M=[Mx,My,Mz]T是转矩读数列向量，是传感器的力读数矩阵，k=[x y z a1a2a3]T是常数列向量。取三组不同姿态下的传感器读数，利用最小二乘法即可计算获得常数列向量k的解，即可获得负载重心坐标x,y,z以及常数项ai，计算公式如式(6)所示：

之后，根据机器人末端不同姿态下重力分力在坐标系之间的关系，即可求得传感器的标定值、机器人底座坐标系相对于世界坐标系的变换矩阵，进一步可获得用于实时补偿重力以及传感器实际接触值的计算式。

记世界坐标系为Ob-XbYbZb，机器人的底座坐标系为Or-XrYrZr，机器人末端六维力传感器坐标系为Os-XsYsZs。将世界坐标系原点Ob与机器人基座标系Or设为重合，于是Or-XbYbZb可由坐标系Ob-XbYbZb先绕Ob的Xb轴旋转U角度然后绕Yb轴旋转V角度得到，其转换矩阵可记为式(7)。同时，将机器人传感器坐标系Os相对于底座坐标系Or的转换矩阵记为srR。

考虑末端负载在不同坐标系下的重力分力向量。其中，在世界坐标系Ob中有gb=[0 0-1]T；而在末端系中OsXsYsZs，应用坐标转换可得式(8)。同时，在末端未工作状态下，将式(8)代入式(1)的前三项可得式(9):

在底座坐标系下，取重力的三个分量Grx=GcosU·sinV,Grx=-GsinU，以及Grz=-GcosU·cosV，可将式(9)简化为如下式。式中，F=[FxFyFz]T为传感器读数，R=[srRT I]为位姿变换矩阵，p=[Grx Gry Grz Fx0 Fy 0 Fz0]T为末端负载分量与零力标定值常量。

取三组不同姿态下的传感器读数，利用最小二乘法即可计算获得常数列向量p的解，即可获得底座坐标系下重力分量以及标定零力的值，计算公式如式(11)所示：

将求得的p中的标定零力值以及通过式(6)求得的负载中心坐标与常数项ai的值代入式(4)，即可求得传感器力矩标定值如式(12)所示：

而对于式(11)求得的底座坐标系下的重力分量，由式(13)中机器人坐标系分量与合成矢量之间的几何关系可得绕Xb轴的偏转角U以及绕Yb轴的偏转角V。

将上式求解结果代入式(9)，并将G[cos U·sin V-sin U-cos U·cos V]T这一常量向量由[Grx Gry Grz]T简化表示，式(9)可以改写为下式。

上式(14)中，传感器坐标系相对于底座坐标系的变换矩阵srR可由各关节编码器角度实时计算得到，即传感器坐标系下用于实时补偿的重力分量[Gx Gy Gz]T也可以通过该式实时计算得到。

与此同时，六维力传感器的实际接触值也可以由下式进行实时计算得到：

2.2导纳控制器

导纳控制通过模仿刚体在柔软弹性环境中的行为，使机器人能够对外部力做出响应，进而使机器人与接触物体的接触力与实际期望相匹配，从而实现更安全、柔顺的交互。在按摩机器人中，需要考虑的是末端按摩头与人体皮肤接触过程中的柔顺控制。

图4人体皮肤-末端按摩头接触力模型  下载原图

首先，考虑建立与人体肌肉组织交互的力学模型，该受力模型相对复杂，目前多数研究以Hill等人提出的三个元素肌肉结构力学模型为基础进行分析，肌肉数学模型三单元即收缩元（CE）、串联弹性元（SEE）和并联弹性元（PEE），用此反映肌肉的功能如图4左侧所示。CE代表可以相对滑动的肌浆球蛋白和肌动蛋白纤维，其张力与它们的横桥数目有关；SEE表示肌浆球蛋白和肌动蛋白纤维、横桥、Z盘以及结缔组织的固有弹性，设它为完全弹性体；PEE表示肌纤维膜、肌束膜等结缔组织，表示松弛状态下肌肉的力学性质。这些单元的具体参数往往是非线性的，变化十分复杂的，难以建立准确的量化的数学模型。这里我们将i肌肉端简化为一个复杂力的交互对象，而机器人端简化为一个弹簧阻尼模型。根据导纳控制模型，机器人的末端受力F如式(16)所示：

式中m,c,k分别表示虚拟质量、虚拟阻尼、虚拟刚度，分别表示机器人在t时刻的末端的交互力、加速度、速度和位移。

同样的，机器人末端所受旋转转矩M如式(17)所示：

式中J,b,l分别表示虚拟转动惯量、旋转阻尼系数、转动刚度，分别表示机器人在时刻的末端所受的转矩、角加速度、角速度和位移转角。

考虑到编码器得到的数据都是离散的，以差分形式表达机器人末端的速度、加速度、角速度以及角加速度如式(18)所示：

其中，Δt为获取编码器位置信息的时间周期，spt-1,αpt-1分别表示机器人在t-1时刻的末端的位移和转角，spt+1,αpt+1分别表示机器人在t+1时刻的末端的期望位移。

在实际的机器人控制中，控制对象为每一控制周期新的末端期望位移增量和姿态角度增量，将式(18)分别代入式(16)与式(17)可得下式：

于是用于机器人实际控制的期望位移指令spt+1和期望姿态转角αpt+1如式(20)所示：

式中k,c,m,l,b,J这六个动力学参数将在实验中预先测定并设置。利用机器人控制器的控制接口，通过获取实时的末端位移spt和姿态转角αpt，结合上述公式即可计算出相应的期望位移和转角，从而实现对机器人运动的控制。

2.3基于虚拟力引导的按摩机器人力位控制方法

按摩机器人在按摩过程需要按照预期轨迹运动，而上节中实现的力柔顺控制在一定程度上会影响机械臂末端轨迹运动的准确性。因此，在闭环控制算法的最后一步需要对考虑轨迹因素并予以修正。人机协作中常见的引导操作员在限定区域内运动的方法是通过虚拟夹具实现的，它利用不同形态、不同维度下的轨迹空间约束表示，辅以不同的约束执行函数，以实时计算当前轨迹点所受到的虚拟约束力[14,15]。

参考上述人机协作中轨迹引导的概念，我们对自主按摩机器人的规划轨迹创建如图5的管型的虚拟夹具，并利用线性刚度方程来生成虚拟引导力。其中，s是规划轨迹，xc是末端当前运动点，r是虚拟管道半径，s(xc)是规划轨迹上距离当前运动点的最近点。定义虚拟力场中生成的约束力如式(21)所示，式中k为虚拟力场中自定义的虚拟刚度参数。

图5沿轨迹生成的虚拟夹具

通过上述步骤，本文形成了如图6的按摩机器人的自主闭环控制。一次完整的闭环首先根据当前时刻运动点在基于期望轨迹生成的虚拟力场中根据式(21)生成虚拟力Ftvirtual，然后，通过获取当前位姿及六维力传感器示数，矢量合成导纳控制器的力和力矩输入信号Fst及Mst，再由式(20)生成机械臂控制端的运动输入信号xst+1及αst+1。

图6按摩机器人自主闭环控制方法 

3、实验

3.1重力补偿实验

本实验首先采集六维力传感器在三组不同姿态下的六维力数据，采集数值如表1所示。

表中，α、β、γ分别为末端传感器坐标系相对于机器人基座系绕x、y、z轴的转角角度。Fx,Fy,Fz,Mx,My,Mz分别表示六维力传感器采集到的原始力。随后，通过2.1节推导出的关系式，解耦出末端工具的重心坐标与重力值，以及六维力传感器的零力值如表2所示。  

表1三组不同姿态下六维力数据  

表2末端重心坐标、重力值与六维力传感器零力值  

将上述数据代入重力补偿算法中，通过将传感器末端置于不同姿态中，可以看到机器人重力补偿的效果。如图7所示，可以看到传感器在Y轴方向和Z轴方向受到了较大的分力。通过重力补偿后的读数如图7下所示，重力补偿后可使得系统获取到精准的用户施加力。

图7重力补偿前后效果  

3.2力位控制实验

为了验证虚拟力引导的力位控制策略的康复效果，我们通过在我们所设计的实验平台中施加一个期望轨迹，通过轨迹随动过程中对比有无虚拟力来对算法的计算进行对比。

首先，对施加一组传统的定轨迹控制方式，实验结果如图8所示。

图8传统定轨迹控制结果  下载原图

之后，本文施加了一组带有虚拟力引导的导纳控制的按摩控制，实验结果如图9所示。

图9带有虚拟力引导的导纳控制结果  下载原图

可以看到，相比于定轨迹的按摩控制，引入虚拟力引导的力位控制策略具有更好的恒力按摩随动性，针对康复部位的按摩力也更加柔顺自然。

4、结语

本文通过在传统的导纳控制系统引入一种虚拟力引导策略，使得按摩机器人具有更好的恒力跟随适应性。该控制策略主要由重力补偿模块、导纳控制模块以及虚拟力引导模块组成，其中重力补偿模块用于补偿不同姿态下末端负载重力对机械臂控制性能的影响，并从六维力传感器中正确获取与患者的接触力。导纳控制模块通过建立“按摩头—皮肤”交互模型，推导并得出当前时刻受力与下一时刻运动控制输入关系式，以实现与外界的柔顺力交互。虚拟力引导模块依据规划轨迹生成虚拟力场，结合当前末端位置生成虚拟引导力，以实现更精准的轨迹跟踪。

参考文献:

[1]杨善岚,杨紫飞,刘斌,等.中青年白领健康问题现状分析及其对策[J].中国疗养医学,2021,30(11):1127-1130.

[2]王水成,王敏.电脑办公与人体保健探究[J].办公自动化,2014(8):56-58.

[3]路来冰,李进,杨少雄.机器人在体育领域的应用现状[J].河北体育学院学报,2022,36(6):15-25.

[4]陶永,刘海涛,王田苗,等.我国服务机器人技术研究进展与产业化发展趋势[J].机械工程学报.2022,58(18):56-74.

[9]高焕兵,鲁守银,王涛,等.中医按摩机器人研制与开发[J].机器人,2011,33(5):553-562.

[15]姜通维,姜勇.基于虚拟力引导的人机协同目标抓取方法[J].智能系统学报,2021,16(04):683-689.

文章来源:高箴,韩栋明,王奕儒等.基于导纳控制与虚拟力控制的运动员康复按摩机器人[J].制造业自动化,2023,45(11):212-216.