基于导纳控制的双边康复机器人运动辅助分析

蔡华年， 费思先， 张忆晨， 孙青， 郭帅， 宋韬

1.上海大学工程技术训练中心，上海市 200444；2.上海大学机电工程与自动化学院上海市智能制造及机器人重点实验室，上海市 200444；3.上海金矢机器人科技有限公司，上海市 200444

0 引言

随着全球老龄化以及运动功能障碍患者数逐步上升[1-4]，传统的一对一人工辅助康复已经很难满足庞大的康复需求[5]。康复机器人以其高重复性、高精度和良好的交互性，能够为患者提供高效和舒适的康复体验[6-10]。但目前研发的上肢康复机器人大多仍需要康复医师现场指导或参与[11-13]，无法完全解决康复医师资源短缺的问题[14-15]。引入机器人远程操作系统，设计有效的双边机器人交互控制策略，可以缓解康复医师资源紧张，提高康复训练效果[16-17]。

近年来，机器人远程操作系统已广泛用于许多应用中，如微创远程手术、远程康复和远程超声[18-19]。双边远程操作系统可以提供远程康复环境[20-21]，允许康复医师和患者交互，以进行各种运动和功能治疗[22-23]。目前主要以人与机器人之间的交互力，以及机器人的速度、位移等运动信息构成机器人之间的控制反馈[24-29]。处理好机器人的位置控制和力控制，是提高机器人辅助康复训练效果的关键[30-31]。

Lanini 等[32]提出一种基于比例微分控制器的单边遥操作以及基于柔顺控制和转矩反馈的双边遥操作，证明控制系统的稳定性，医师能够实时感受患者参与康复训练的积极性。Carignan 等[33]针对交互过程中网络时延问题，提出一种基于阻抗控制和波动变量的协同控制结构，实现对于期望轨迹的稳定实时跟踪，并减少网络通讯延迟，提高康复训练的安全性。Cho等[34]针对控制系统动态模型的不确定性，提出一种基于滑模控制的阻抗控制器作为从控制器，证明具有良好的鲁棒性，辅助的准确性提高。Sharifi 等[35]提出一种协同遥操作系统向多客户端系统转换的框架，该框架以期望的力在机器人的所有力之间进行协调，通过增加通信通道，实现多主/多从结构的协同遥操作，提高机器人康复训练效率。但该系统少有应用于多自由度的末端牵引式康复机器人，且控制策略缺少对机器人末端姿态的保护，可能会对患者造成二次伤害，存在一定的安全隐患。

本研究提出一种机器人主端速度导纳、从端位置控制与姿态导纳相结合的控制策略。将远程操作系统应用于两台六自由度末端牵引式康复机器人，采用远程示教模式，康复医师在远端拖动主机器人，引入速度导纳控制生成示教轨迹，从机器人接收主机械臂的位置信息并通过位置环进行轨迹跟踪；此外，针对患者端产生交互力矩过大的问题，从机器人采用末端姿态导纳控制，确保患者手臂时刻处于安全位置。

1 双机远程康复框架

1.1 导纳控制原理

该双机远程操作框架中交互控制主要采用导纳控制：

其中M、D、K均为正定对角矩阵，分别代表虚拟惯量、阻尼和刚度矩阵；X、Ẋ、Ẍ分别表示位置、速度和加速度。X0表示平衡位置，Fext表示环境的外力。

采用远程示教模式时，主机器人端虚拟刚度K=0，平衡位置X0=Ẋ0=Ẍ0= 0。

通过力传感器测得人手臂与机械臂的交互力Fs作为环境的外力，该动力学方程可表示为：

机械臂所遵循的轨迹可以规定为期望的位置或速度。通过拉普拉斯变换，期望速度的传递函数为：

其中Ẋd(s)是Ẋ的拉普拉斯变换，Fs(s)是Fs的拉普拉斯变换，表示由操作者施加的外力矢量，s是拉普拉斯算子。

通过双线性变换将上述传递函数离散化，离散化后的期望速度为：

其中fs(k)是时间步长k处的交互力，Ẋd(k)是时间步长k处的期望速度，Ts是采样时间。在时间步长k处的期望加速度为：

对期望速度进一步积分，可得机械臂在时间步长k处的期望位置：

通过公式(4)和(6)，可分别实现机械臂的速度控制和位置控制。

1.2 操作平台和通讯

双机远程操作平台需要从机器人能够跟随康复医师控制的主机器人进行相应的康复训练(图1)。选用两台搭载6R 关节型机械臂UR5 的上肢康复机器人，支持自碰撞检测，保证人机协作对安全性的要求。采用Modbus、TCP/IP 等通信协议与外部进行通信。该上肢康复机器人还包括六维力传感器(OnRobot，采样频率500 Hz)、工作站和交互设备。

图1 末端式上肢双边康复机器人平台示意图

机械臂和控制柜通过以太网和工作站通讯实现双向数据传输。力矩传感器采集交互力信息，将数据发送给工作站。交互设备与工作站连接，交互界面供操作者下达指令。服务器负责进行数据中转，两台上肢康复机器人与服务器间采用TCP/IP通讯协议进行数据传输。客户端给服务端发送连接请求，服务器接受请求后，客户端和服务端建立连接。在康复医师示教模式下，服务器接收主工作站发来的位置数据，并将其转发至从工作站。

采用服务器进行数据中转而不是直接传输数据，不需要对主、从机器人做严格规定，任意一台上肢康复机器人都可以用作主机器人或从机器人。该平台也适用于后续对两台上肢康复机器人进行双机协作模式的开发。

2 控制算法

在远程示教模式下，康复医师在远端控制主机器人进行康复轨迹示教，从机器人带动患者进行轨迹跟踪并使其末端可调节。为实现该功能，提出一种双边导纳控制策略(图2)。

图2 远程示教模式控制策略图

在主机器人端，通过末端六维力传感器测得医师与机器人的交互力/力矩Fd，通过主端速度导纳模型，将Fd转化为机械臂末端速度Vd：

其中，Fd(s)是医师施加给机械臂的外部力/力矩，Vd(s) 是机械臂的末端期望速度，Fd(s) ∈Rm、Vd(s) ∈Rm，m是六维力传感器的输出矢量的维数。Md、Bd分别为主端速度导纳模型的惯量和阻尼，Md∈Rm、Bd∈Rm×m。所以，末端期望速度通常具有6个分量:

其中，v是末端线速度，v=[vx vy vz]T，ω是末端角速度，ω=[ωx ωy ωz]T，f是力/扭矩传感器的力，f=[fx fy fz]T，τ是力/扭矩传感器的扭矩，τ=[τx τy τz]T。

通过逆雅可比矩阵将机械臂末端的线速度和角速度转化为机械臂关节角速度：

其中，q̇d是期望的关节角速度，J-1(q)是机器人的逆雅可比矩阵。

将q̇d下发给机械臂，从而实现主机器人按照康复医师拖动示教的轨迹运动。机械臂实时读取自身关节角，根据机械臂正运动学可将其转化为机械臂的实时位姿Xd：

其中，pd=[xd yd zd]T，xd、yd、zd是主机器人的位置，Rd=[αd βd γd]T，αd、βd、γd是主机器人的姿态。

仅将主机器人的位置[xd yd zd]T发送给服务器，服务器接收后将其发送给从机器人。

从机器人端接收到主机器人的位置后，通过从端导纳模型，将患者与从机械臂的交互力矩τp转化为从机械臂的末端姿态θp：

其中，τp(s)是患者施加给机械臂的外部力矩，θp(s)是发送给机械臂的末端期望姿态，θp(s) ∈Rm、τp(s) ∈Rm，m是六维力传感器的输出力矩矢量的维数。Mp∈Rm×m、Bp∈Rm×m、Kp∈Rm×m分别为主端速度导纳模型的惯量、阻尼、刚度。所以，末端期望姿态有3个分量:

其中，Rp=[αp βp γp]T,αp、βp、γp是机器人末端的姿态，τp=[τx τy τz]T,τx、τy、τz是六维传感器的扭矩。将该姿态与服务器发送过来的主机器人位置进行叠加，得到从机械臂的末端位姿：

将从机械臂的末端位姿通过逆运动学转换为机械臂的实时关节角度qp，并将其下发至从机械臂，并通过位置环控制带动患者运动。

3 实验验证

3.1 实验搭建

实验结合末端式上肢双边运动平台与基于Unity的视觉反馈场景(图3)。训练任务是康复医师带动受试者完成特定的二维空间轨迹圆，圆心分别为0 mm、500 mm、-50 mm，半径120 mm。通过视觉反馈场景能帮助康复医师和受试者感知运动空间轨迹。黄点代表康复医师或受试者手的位置，白色曲线代表目标轨迹，绿点代表圆心，左侧显示条代表康复医师或受试者与机器人末端的交互力。

图3 远程示教模式实验测试场景

招募3 例男性健康受试者，平均年龄(25.33±2.3)岁，身高(1.72±0.1) m，体质量(65.6±2.9) kg。康复医师和受试者均将右手臂放在机器人的末端手柄上。

康复医师带动受试者完成8 圈圆轨迹运动训练。前4圈不对患者端进行导纳控制，从机器人末端固定；后4圈对患者端添加姿态导纳控制。控制参数如下：

实验前，受试者执行一些简单任务，如画直线，直到他们熟悉上肢康复机器人训练系统。实施硬件和软件安全保护，确保受试者安全。

采集医师端和患者端的位置、速度，以及患者端的交互力和力矩，通过高斯低通滤波器(频率100 Hz)对数据进行处理。为评估受试者轨迹跟踪能力，引入平均轨迹误差MAE：

其中，N为采样点的数目，ppat为受试者在采样点i处的位置，pdoc为医师在采样点i处的位置。

3.2 结果

在导纳控制和固定姿态下，患者端均具有良好的轨迹跟踪效果。固定姿态下，实时轨迹误差0.32～0.64 mm；可变姿态下，实时轨迹误差0.38～0.98 mm。在固定姿态和可变姿态下，平均轨迹误差相差不大(图4)。

图4 各受试者平均轨迹误差

相较末端固定姿态控制，位置控制与末端姿态导纳相结合的控制方法下，患者端平均轨迹误差与最大轨迹误差分别增加0.06 mm 和0.34 mm，均在合理范围内，能够满足远程示教模式的需求，在追踪精度、追踪平稳性和追踪速度方面，具有良好性能。

患者端可变姿态下，受试者对机械臂交互力和交互力矩均较固定姿态下下降；固定姿态下，交互力与交互力矩在运动到某些位置时存在突变。增加姿态导纳控制后，交互力与交互力矩变异范围减少，人机交互的安全性和舒适性提高(图5～图7)。

图5 固定姿态和可变姿态下的交互力和交互力矩

图6 各受试者平均交互力

图7 各受试者平均交互力矩

4 结论

本研究面向远程操作系统提出一种基于导纳控制的双边康复机器人运动辅助策略，能使康复医师不在现场时，也能及时为患者提供康复治疗，方便患者在家中进行康复训练。本研究针对远程示教模式，引入两种导纳控制器。主端设计基于速度的导纳控制器，将康复医师与主机器人的交互力转化成速度，通过速度环对主机械臂进行运动控制；从端设计基于姿态的导纳控制器，在对从机械臂进行位置控制的同时，通过导纳控制将患者与从机械臂的交互力矩转换为姿态角，达到从机械臂末端柔顺的效果。

实验结果初步表明，从机械臂带动患者运动时，能够准确跟踪医师端的轨迹，并实现对从机械臂末端姿态的柔顺控制，避免机械臂末端姿态固定导致患者手臂不适的情况。

利益冲突声明：所有作者声明不存在利益冲突。