一种上肢康复机器人的重力补偿方法

技术领域

本发明属于机器人重力补偿相关技术领域，更具体地，涉及一种上肢康复机器人的重力补偿方法。

背景技术

机器人重力补偿参数求解是机器人控制非常重要的一步，尤其对于串联式上肢康复机器人而言，先对机器人本体重力进行补偿是识别患肢运动意图的必要步骤。重力补偿参数求解方法主要有被动补偿和主动补偿两种，被动补偿通过设计者的经验或者是理论计算得到机器人关节负载的最大重力值，然后选择能够平衡关节负载最大重力的一定比例的配重块或者是辅助弹簧，最后通过设计简单的配重结构便可以实现对重力一定程度的补偿。主动补偿主要是通过建立机器人动力学方程，然后通过三维软件仿真得到各个关节的重力，最终通过实时改变前馈力矩补偿的值，便可以在任意位置实现对重力矩的补偿，主动补偿相对于被动补偿的优势在于它可以补偿任意位置的重力矩。

但是，上述两种方法都存在一些没有解决的问题：由于上肢机器人在运动的过程中，各个关节所受重力随运动位置的不同而不同，被动补偿无法补偿任意位置的重力；通过建立动力学模型的，在三维软件中推导出重力补偿参数值的主动补偿方法，没有考虑机器人的结构在加工装配中的精度以及材料的分配不均匀，从而直接从软件中仿真得到的模型参数与实际偏差较大。

专利CN201810331540中公开了一种工业机器人柔性伺服控制方法，其将加速度项、速度项和位置项耦合在一起，综合考虑，依次求解了速度、加速度带来的影响，这些影响都是不能精确求得的，所以该方法求得的结果是很不准确的，并且建模复杂，计算量大，实际使用前景不理想；

CN201511030390中公开了一种触觉反馈装置及其变阻尼控制方法和应用，将重力补偿的力矩作为已知量，并没有对重力补偿的步骤进行阐述，即其内容讲述的并非是机器人的重力补偿，它是强调的控制方法。上述两个专利中要么实现的是加速度项、速度项和位置项耦合建立的重力补偿模型，要么没有公开具体的重力补偿方法；因此，急需一种根据机器人各关节位置与重力的函数关系，建立仅考虑重力矩项的模型的补偿方法，以解决现有被动补偿方法不能补偿任意位置的重力，主动补偿方法不能准确建模的技术难题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种上肢康复机器人的重力补偿方法，通过将加速度项、速度项、和位置项解耦合，减少采集过程中多变量的累积误差，提高补偿精度，降低计算复杂度，同时将摩擦力与重力解耦合，极大的提高了重力参数的辨识精度和重力运算速度。

为实现上述目的，按照本发明的一个方面，提供了一种上肢康复机器人的重力补偿方法，包括下列步骤：

S1对于包括多个关节的上肢康复机器人，构建该机器人的运动轨迹，采集运动轨迹中不同位姿处每个关节的力矩和位姿；

S2建立世界坐标系并以该世界坐标系作为基坐标系，对于每个关节，建立每个关节各自的关节坐标系并求解各自坐标系中的重力加速度g，求解构建每个关节坐标系与所述基坐标系的旋转矩阵R和雅克比矩阵J；利用雅克比、旋转矩阵和重力加速度，构建以关节的位姿为输入，关节的力矩作为输出的预测模型；

S3利用步骤S1中采集的力矩和位姿的数据计算所述预测模型中的待辨识系数，改变所述机器人的位姿，测量当前位姿下每个关节的实时力矩和实时位姿，利用所述实时位姿和预测模型计算获得预测力矩，所述实时力矩减去该预测力矩即实现该机器人的重力补偿。

进一步优选地，在步骤S2中，所述预测模型按照下列关系式进行：

B＝AX

其中，B是每个关节的力矩，A是关于关节位姿的回归方程系数矩阵，X是待辨识的系数。

进一步优选地，所述回归方程系数矩阵A按照下列关系是进行：

A＝JK

其中，J是关节坐标系与基坐标系之间的雅克比矩阵，K是权重矩阵。

进一步优选地，所述K按照下列关系式进行：

其中，[0g

进一步优选地，在步骤S3中，所述计算所述预测模型中的系数采用带惩罚项的最小二乘法求解。

进一步优选地，采用带惩罚项的最小二乘法求解按照下列关系式进行：

X＝(A

其中，B是每个关节的力矩，A是关于关节位姿的回归方程系数矩阵，X是待辨识的系数，λ是惩罚系数，I是单位矩阵。

进一步优选地，在步骤S1中，采集每个关节的力矩和位姿后，还需对采集的数据进行滤波平滑处理，以此转化成相应的矩阵形式。

进一步优选地，在步骤S1中，所述构建该机器人的运动轨迹中的运动轨迹为仿真的人体运动的轨迹，均为负载端速度低于5°/s的轨迹。

按照本发明的另一个方面，提供了一种上述所述的重力补偿系统，该系统包括轨迹设计模块、数据采集模块、数据处理模块、重力参数辨识模块和实时补偿模块，其中：

所述轨迹设计模块用于设计机器人的运动轨迹；

所述数据采集模块用于采集机器人运动轨迹中不同位姿处每个关节的力矩和位姿；

所述数据处理模块用于将采集的数据抽取出来，然后进行滤波平滑处理，转化成对应的矩阵格式；

所述重力辨识模块用于构建预测模型，并利用所述数据处理模块处理后的矩阵格式数据进行重力补偿参数辨识，以此确定最佳的待辨识参数；

所述实时补偿模块用于利用测量的实时力矩和预测力矩相减，对重力状态进行补偿，实现无重力状态。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具备下列有益效果：

1.本发明通过将加速度项、速度项、和位置项解耦合，即建立的模型中，不用再去考虑速度与加速度，直接获取与重力矩有关的信息——位姿和力矩，提高了辨识精度；相比与现有的通过采集加速度和速度项建立与重力矩的关系，减少引入变量的数量，并且也避免采集加速度和速度的过程本身中产生的误差，进而减少最终结果中的累积误差，提高补偿精度，降低计算复杂度；

2.本发明中之所以仅仅选择选择位姿项和力矩构建预测模型，原因主要包括两个方面，首先，如图5所示，本发明中的力传感器设置在减速器的输出端，该力传感器在测量过程中不能测量减速器的重量，机器人关节中摩擦力的来源主要是减速器带来的摩擦力，由于不能测量减速器的重量，故不用考虑机器人关节内部的摩擦力，实现摩擦力与重力的解耦合，另外，本发明中机器人关节运动的轨迹是模拟人的运动轨迹，均为低速轨迹，其向心力和加速度都很小，因此，不用采集加速度和速度的信息建立模型，仅仅采用位姿项即可；

3.本发明中建立了位姿与重力之间的模型，B＝AX，A是与位姿有关的矩阵，B是各关节力矩，模型表达的一一映射的关系，一组关节角位姿对应一组关节力矩，不同关节角位姿对应不同力矩，通过建立任意位置对应的力矩的重力模型保证任意位姿的重力都可以被补偿，解决现有技术中被动补偿方法不能补偿任意位置的重力问题。

附图说明

图1为本发明实施例提供的一种上肢康复机器人的重力补偿方法流程框图；

图2为本发明实施例提供的上肢康复机器人三维模型示意图；

图3为本发明实施例提供的上肢康复机器人坐标系示意图；

图4为本发明实施例提供的上肢康复机器人各处理模块之间的关系图；

图5为本发明实施例提供的上肢康复机器人腕部屈伸关节传动链的示意图；

图6为本发明实施实例提供的上肢康复机器人重力回归模型运行过程中，力矩传感器实测值与预测值偏差，其中，(a)是上肢康复机器人肩部第一个关节重力补偿力矩信息；(b)是肩部第二个关节重力补偿力矩信息；(c)是肩部第三个关节重力补偿力矩信息；(d)是肘部前屈/后伸关节重力补偿力矩信息；(e)是腕部内旋/外旋关节重力补偿力矩信息；(f)是腕部前屈/后伸关节重力补偿力矩信息；(g)是腕部外展/内收关节重力补偿力矩信息。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，一种上肢康复机器人的重力补偿方法，包括下列步骤：

S1对于包括多个关节的上肢康复机器人，构建该机器人的运动轨迹，采集运动轨迹中不同位姿处每个关节的力矩和位姿；

S2建立世界坐标系并以该世界坐标系作为基坐标系，对于每个关节，建立每个关节各自的关节坐标系并求解各自坐标系中的重力加速度g，求解构建每个关节坐标系与所述基坐标系的旋转矩阵R和雅克比矩阵J；利用雅克比、旋转矩阵和重力加速度，构建以关节的位姿为输入，关节的力矩作为输出的预测模型；

S3利用步骤S1中采集的力矩和位姿的数据计算所述预测模型中的系数，改变所述机器人的位姿，测量当前位姿下每个关节的实时力矩和实时位姿，利用所述实时位姿和预测模型计算获得预测力矩，所述实时力矩减去该预测力矩即实现该机器人的重力补偿。

在步骤S2中，所述预测模型按照下列关系式进行：

B＝AX

其中，B是每个关节的力矩，A是关于关节位姿的回归方程系数矩阵，X是待辨识的系数。

所述回归方程系数矩阵A按照下列关系是进行：

A＝JK

其中，J是关节坐标系与基坐标系之间的雅克比矩阵，K是权重矩阵。

所述K按照下列关系式进行：

其中，[0g

在步骤S3中，所述计算所述预测模型中的系数采用带惩罚项的最小二乘法求解。

采用带惩罚项的最小二乘法求解按照下列关系式进行：

X＝(A

其中，B是每个关节的力矩，A是关于关节位姿的回归方程系数矩阵，X是待辨识的系数，λ是惩罚系数，I是单位矩阵在步骤S1中，采集每个关节的力矩和位姿后，还需对采集的数据进行滤波平滑处理，以此转化成相应的矩阵形式。

在该方法中，将机器人每个关节传动链划分为两部分，第一部分为电机轴输出端到力矩传感器输入端，第二部分为力矩传感器输出端到负载端，在机器人的运动过程中，摩擦力主要存在于第一部分，重力存在于第二部分，因此，在对关节所受重力补偿参数进行辨识时，便可以只单独考虑第二部分，使得摩擦力与重力解耦合。

建立了康复机器人的坐标系，根据每个坐标系之间的旋转矩阵，计算每个关节相对于基座标系的雅可比矩阵的转置，进而建立每个机器人每个关节受其他关节重力影响的表达式；最终建立康复机器人的重力回归方程。

如图4所示，本发明中重力补偿方法中包括多个模块，其中为轨迹设计模块、数据采集模块、数据处理模块、重力参数辨识模块和实时补偿模块，轨迹设计模块设计了康复机器人低速的拟人激励轨迹，保证了运动缓慢平稳，排除速度与加速度对计算的干扰。

在数据采集模块采集康复机器人执行激励轨迹中的大量数据信息，利用数据处理模块对数据进行处理，将处理后的数据信息使用参数求解模块可以准确预测各个关节的重力矩，最后通过在重力补偿参数辨识模块进行运算，得到最终的每个关节位姿对应的力矩传感器偏置值。

在重力补偿参数辨识模块使用了岭回归损失函数，同时求出了重力补偿参数。最后重力实时补偿模块根据预测的实时力矩与实测的力矩作差，当二者之间的差值为0时，实现重力的完全补偿。

下面将结合具体的实施例进一步说明本发明。

本申请提出的一种上肢康复机器人的重力补偿方法由图1给出了实现流程，包括步骤S1-S4步骤，其中：

步骤S1：将康复机器人上电后，上位机从数据库中加载预先编写好的“摸嘴巴”运动轨迹，加载完成后，下发执行命令控制机器人进行运动该轨迹10次。

当“摸嘴巴”的运动轨迹执行了10次后，再依次从数据库中加载“摸头”，“摸右耳”，“摸左耳”，“摸左肩”，“摸右肩”，“摸肚”6条激励轨迹各执行上述步骤一次。

实际使用中，可以改变各关节执行激励轨迹的顺序和次数。

需要说明的是，在实施步骤S1之前，首先需要设计好激励估计，具体的，需要根据图2的上肢康复机器人三维模型示意图建立好上肢康复机器人的坐标系如图3所示，基座标系{0}原点位于肩部三个关节轴线交汇的位置。

通过VICON采集人体自然运动的关节数据，然后将人体各关节运动数据映射到康复机器人的各个关节空间，然后，对关节轨迹进行插值，插值得到的速度较小，将插值得到的轨迹作为关节运动时的激励轨迹。

步骤S2:在康复机器人执行激励轨迹时，通过数据采集模块采集机器人运动过程中各种传感器的数据，包括霍尔传感器返回的电流，力矩传感器返回的力矩，位置传感器返回的负载端位置值，然后减去对应的起始位置的初始值后，保存在数据库中，供步骤S3中的数据处理模块进行使用。

需要说明的是，在实施步骤S1之前，首先需要确定力矩传感器在轨迹起始点的初始值。

步骤S3：从数据库中将数据采集模块采集到的数据加载到数据处理模块中，使用数据处理模块进行处理，将力矩传感器与位置传感器返回的数据抽取出来，然后进行滤波平滑处理，转化成对应的矩阵格式，以便步骤S4的重力参数辨识模块进行使用。

这里的滤波平滑环节采用的是巴特沃斯滤波器，参数通过进行多次实验确定。除了巴特沃斯滤波器外，也可以采用其他的滤波器。

步骤S4:在重力参数辨识模块中，将数据处理模块处理后的矩阵格式数据使用岭回归损失函数对重力回归矩阵进行重力补偿参数辨识，通过多组实验后，确定惩罚项系数的值，最后得到最佳的待辨识参数。

步骤S5：在实时补偿模块中，将实际运行过程中位置传感器测量得到的关节角作为重力回归模型的输入，得到各个关节力矩的预测值τ

需要说明的是，在实时本方法之前，需要根据机器人学相关理论建立好重力补偿模型。

步骤如图3上肢康复机器人坐标系示意图所示，其中{0}为基座标系。

1)根据所建坐标系建立DH参数表，求解齐次变换矩阵

2)根据转换矩阵求解每个关节相对于基座标系的雅可比矩阵J

3)建立各关节位姿作为输入，各关节力矩为输出的重力模型Y＝AX

建立重力模型详细过程如下：

每个连杆的重力向量在基座标中的描述为

其中g＝[g

设r

由运动学变化公式可知，关节连杆i的重力矩在基座标系中的描述为

关节i的重力与重力矩在基座标系的描述如式(4)所示。

由力域雅可比理论，结合式(3)、(4)可得式，可得关节j的重力对关节i的力矩影响的表达式，如所示。

其中，τ

其中[

其中，[

康复机器人每个关节受到的机器人本体重力可以看成是其他关节重力在该关节上的作用之和，结合，可以得到康复机器人每个关节需要平衡的重力矩，如式(9)所示

其中，

即可得式(11)

Y＝AX (11)

该方程为欠定方程，可以采用最小二乘法来确定最优解。

通过采集机器人在各关节不同位置的绝对式位置传感器和力矩传感器的反馈值，可以得到如(12)式的矩阵方程

为了防止出现过拟合，采用正则化技术，即采用岭回归的代价函数：

当康复机器人装配完成后，关节i的质量m

X的解析解为

X＝(A

经过理论推导，本发明提出的一种上肢康复机器人重力补偿参数的辨识方法，可以提高机器人的辨识精度和计算速度。

下表1是本实例中求出的上肢康复机器人的待辨识连杆质量参数X，第一列表示的康复机器人的关节编号，第二列到第七列表示的是经过模型辨识得到列向量X的值。具体说，i行j列表示的是列向量X的第7*(i-1)+1个元素的值。

图6表示的是本实例中，蓝色曲线为本重力补偿方法预测的重力矩，红色点画线为力矩传感器是测距，黄色虚线为模型预测值与实际测量值的差值，差值较小，表明预测结果较好。曲线依次表示本实例机器人的七个主动关节，图6中(a)肩部第一关节，(b)肩部第二关节，(c)肩部第三关节，(d)肘部前屈后伸关节，(e)腕部内旋外旋关节，(f)腕部前屈后伸关节，(g)腕部外展内收关节。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。