CN115778752A - 一种用于坐卧式康复机器人中可抑制抖动的零力控制方法 - Google Patents

Abstract

本发明涉及一种用于坐卧式康复机器人中可抑制抖动的零力控制方法，包括以下步骤：S1、根据关节检测模块得到的关节位置，速度，力矩构建出动力学模型；S2、基于动力学模型，关节力矩传感器实时反馈力矩得到外界接触力矩；S3、根据步骤S2中得到的外界接触力矩计算得到速度指令；S4、通过关节角度编码器捕捉运动中由于惯性产生的晃动角度，计算得到速度修正量，并补偿到步骤S3中得到的速度指令，消除抖动，实现零力控制。本发明解决了现有基于关节力矩反馈的零力控制精度较低，谐振现象容易发生等的不足。

Description

一种用于坐卧式康复机器人中可抑制抖动的零力控制方法

技术领域

本发明涉及机器人控制技术领域，具体涉及一种用于坐卧式康复机器人中可抑制抖动的零力控制方法。

背景技术

坐卧式康复机器人，适用于术后初期的下肢康复训练患者。这个阶段的患者虽然能够自主坐起，但仍然无法支撑站立，需要坐卧式康复机器人处于助力模式，即被动运动模式，但是为了鼓励患者发挥更多的主观能动性，即鼓励患者主动抬腿，当患者肌力能够支撑其自体腿重的时候，康复机器人只需要完成跟随运动，此时即为零力控制。目前零力控制主要有两种实现方法：基于力矩控制的零力控制与基于位置控制的零力控制技术。

基于力矩控制的零力控制通过将检测到的关节角度偏差转换为相应的力矩指令从而实现零力控制，由于首先需要克服减速器的静摩擦力才能得到电机输出轴的关节角度偏差，这使得在静摩擦力较大或重载的场合难以取得良好的柔顺效果。

基于位置控制的零力控制通过将检测到关节外力与动力学模型计算得到的力相加并转换为相应的位置指令，从而实现零力效果。由于需要得到关节外力矩，目前研究较多的有3种获取方式：电机驱动器电流读取，末端六维力传感器获取，关节扭矩传感器获取。第一种方法无需安装额外的力传感器，成本最低，但是机器人关节处往往经过多级减速，通过驱动器估计外力矩的方式不论是响应速度还是感知精度都难以保证。第二种方法是目前机器人实现零力控制最普遍的方法，但是由于力检测只能通过末端六维力传感器实现的，所以机器人无法感知其他位置的人机交互，这也限制了这种方法的使用场合。第三种方法需要在机器人各个关节安装单轴力矩传感器，相较于第一种方法可以很好地解决静摩擦力带来的干扰问题，相较于第二种方法可以很好地实现机械臂上任意位置的零力拖动，然而实际中主流协作机器人很少使用这种方式进行力控研究，主要原因是这种感知方法依赖于关节力矩传感器，增加了关节机械结构的复杂程度，并且当负载突然消失或空载快速停止时，机械臂会因为长时间找不到平衡点而出现不断抖动的现象，影响柔顺性。在康复初期，由于患者的出力是随机的，需要康复机器人在助力模式和跟随模式间不断切换，这也使得基于关节力矩反馈的零力控制抖动现象更加明显。

发明内容

为了克服现有基于关节力矩反馈的零力控制精度较低，谐振现象容易发生等的不足，本发明提供了一种具有抖动抑制功能的零力控制方法，该方法可应用于带有关节扭矩传感器的坐卧式康复机器人控制系统中，可在抑制关节抖动的同时提高关节柔顺性。

为了实现上述功能，本发明采取的技术方案如下：

S1、根据关节检测模块得到的关节位置，速度，力矩构建出动力学模型。

S2、基于动力学模型，关节力矩传感器实时反馈力矩得到外界接触力矩。

S3、根据步骤S2中得到的外界接触力矩计算得到速度指令。

S4、通过关节角度编码器捕捉运动中由于惯性产生的晃动角度，计算得到速度修正量，并补偿到步骤S3中得到的速度指令，消除抖动，实现零力控制。

进一步的，在本发明提出的用于坐卧式康复机器人中可抑制抖动的零力控制方法里，所述步骤S1包括：

S11、建立机器人动力学模型得到：

其中，M(θ)为惯性项矩阵，

为科氏力及离心力项矩阵，G(θ)为重力项矩阵，F_v为粘摩擦力项矩阵，F_s为静摩擦力项矩阵，

为关节角加速度，

为关节角速度，θ为关节角度，τ为关节力矩，

为与

正负有关的符号函数，τ为关节力矩矩阵。

S12、基于测得的关节力矩，关节角度，角速度，角加速度进行动力学参数辨析，首先对动力学方程进行参数线性化处理得到：

其中，τ为关节力矩矩阵，通过关节力矩传感器获得。

为观测矩阵，P为辨析矩阵，需要通过辨析得到，P＝[p₁,p₂,p₃,…,p_i]，其中p_i＝f(I_i,r_i,l_i,m_i,f_vi,f_si)，其中I_i为连杆i转动惯量，r_i为连杆i质心距离，l_i为连杆i的长度，m_i为连杆i质量，f_vi为连杆i动摩擦力系数,f_si为连杆i静摩擦力系数。

S13、将S12中辨析得到的动力学参数代入S11，得到动力学方程。

进一步的，在本发明提出的用于坐卧式康复机器人中可抑制抖动的零力控制方法里，所述步骤S2包括：

S21、将关节检测模块得到的实时关节位置θ_t，实时速度

实时加速度

代入S13动力学方程中，得到关节计算力矩τ_t：

S22、根据关节检测模块得到的关节实际反馈力矩τ_e与S21得到的关节计算力矩τ_t计算得到外界接触力矩τ_ext：

τ_ext＝τ_t-τ_e

进一步的，在本发明提出的用于坐卧式康复机器人中可抑制抖动的零力控制方法里，所述步骤S3包括：

S31、根据步骤S22中得到的外界接触力矩τ_ext，建立力矩转速转换关系，将其转换为关节目标速度指令

其中，k₁表示力矩速度映射系数，sign(τ_ext)为与τ_ext正负有关的符号函数。

S32、为了避免微小的抖动也被控制系统捕捉，使系统过于敏感，需要设置接触力矩触发阈值，保证系统稳定性，可以得到：

其中，τ₁,τ₂表示外界接触力矩触发阈值，可以根据不同的操作场景与需求进行调节，可以为常数或者时变参数。

S33、考虑到人机协作系统中的安全，在外界接触力矩大于某个阈值时应考虑是否发生碰撞等特殊情况，这时机器人会控制机械臂缓慢回退直到外界接触力矩回到正常范围，可以得到：

其中，k₂为回退速度，τ₃,τ₄为外界接触力矩最大阈值，可以根据不同的操作场景与需求进行调节，可以为常数或者时变参数。

进一步的，在本发明提出的用于坐卧式康复机器人中可抑制抖动的零力控制方法里，所述步骤S4具体如下：

关节处编码器实时记录关节实际角速度

并每隔一个采样周期T检测机械臂抖动情况，若在采样周期内

发生多次正负变化说明抖动发生，则选取最近一个未发生抖动周期内的关节角速度

作为关节目标速度指令

直到抖动消失。

本发明具有以下有益效果：本发明通过关节检测模块得到的关节位置，速度，力矩，进行动力学参数辨析并构建出动力学模型，通过将动力学模型计算得到的关节力矩与关节力矩传感器反馈的实际力矩计算得到人机接触力，实现了机械臂上任意位置的接触力检测并减小了静摩擦力的干扰。相较于电流检测，关节零力控制效果更好；相较于六维力传感器，适用场合更加灵活。采用速度控制而非直接转矩控制，提高了系统的安全性；采用速度控制而非位置控制，提高了系统的响应速度。设计关节抖动补偿模块，解决了现有基于关节力矩反馈的零力控制精度较低，谐振现象容易发生等的不足。

附图说明

图1是本发明提供的机器人零力控制方法的流程图；

图2是本发明提供的机器人零力控制方法的控制框图。

具体实施方式

为了加深读者对本发明的理解，下面将结合附图和具体实施方式对本发明的具体实施方式进行详细说明。以下附图及实施方式用于说明本发明，但不用来限制本发明的范围。

如图1所示，一种用于坐卧式康复机器人中可抑制抖动的零力控制方法的流程图，包括以下步骤：

S1、根据关节检测模块得到的关节位置，速度，力矩构建出动力学模型。

S2、基于动力学模型，关节力矩传感器实时反馈力矩得到外界接触力矩。

S3、根据步骤2中得到的外界接触力矩计算得到速度指令。

S4、通过关节角度编码器捕捉运动中由于惯性产生的晃动角度，计算得到速度修正量，并补偿到步骤3中得到的速度指令，消除抖动，实现零力控制。

在所述步骤1中包含以下3个部分，详述如下：

S11、首先需要得到机器人动力学模型，综合考虑到机器人运动过程中的各种受力情况，惯性力，科氏力，离心力，重力，摩擦力建立机器人动力学通解模型：

其中，M(θ)为N维惯性项矩阵，

为N维科氏力及离心力项矩阵，G(θ)为N维重力项矩阵，F_v为N维粘摩擦力项矩阵，F_s为N维静摩擦力项矩阵，

为N维关节角加速度，

为N维关节角速度，θ为N维关节角度，τ为N维关节力矩，

为与

正负有关的符号函数，τ为N维关节力矩矩阵。

S12、在得到动力学通解模型后，首先需要对公式中的参数进行辨析，记录不同关节角度，角速度下的关节力矩进行动力学参数辨析。首先对动力学方程进行参数线性化处理得到：

其中，τ为关节力矩矩阵，通过关节力矩传感器获得。

为观测矩阵，P为辨析矩阵，需要通过辨析得到，P＝[p₁,p₂,p₃,…,p_i]，其中p_i＝f(I_i,r_i,l_i,m_i,f_vi,f_si)，其中I_i为连杆i转动惯量，r_i为连杆i质心距离，l_i为连杆i的长度，m_i为连杆i质量，f_vi为连杆i动摩擦力系数,f_si为连杆i静摩擦力系数。通过将不同不同关节角度，角速度下的关节力矩代入辨析函数可得到关于转动惯量，质心距离，连杆长度，连杆质量，连杆关节动摩擦力系数，连杆关节静摩擦力系数的辨析矩阵P。

S13、将S12中得到的辨析矩阵P代入S11得到的动力学通解公式，得到机器人动力学模型。

在步骤1中得到的动力学模型基础上，所述步骤2中包含以下2个部分，详述如下：

S21、能否快速有效地检测出人机的接触力从而判断人的运动意图是零力控制的关键。将关节检测模块得到的实时关节位置θ_t，实时速度

实时加速度

代入S13动力学方程中，得到关节计算力矩τ_t：

S22、根据关节检测模块得到的关节实际反馈力矩τ_e与S21得到的关节计算力矩τ_t计算得到外界接触力矩τ_ext：

τ_ext＝τ_t-τ_e

通过关节力矩传感器与动力学模型计算得到的外界接触力矩可以有效检测出人的运动意图，提高柔顺性及灵敏度。

在步骤2得到的外界接触力τ_ext基础上，所述步骤3中包含以下3个部分，详述如下：

S31、不同于驱动器反馈电流计算得到的实际关节反馈力矩力矩τ_e，通过关节力矩传感器获取的τ_e可以更加精准地估计用户与机器人的接触力，解决静摩擦力带来的困扰。根据步骤S22中得到的外界接触力矩τ_ext，将其转换为关节目标速度指令

其中，k₁表示力矩速度映射系数，sign(τ_ext)为与τ_ext正负有关的符号函数。

S32、为了避免微小的抖动也被控制系统捕捉，使系统过于敏感，需要设置接触力矩触发阈值，保证系统稳定性，可以得到：

其中，τ₁,τ₂表示外界接触力矩触发阈值，可以根据不同的操作场景与需求进行调节，可以为常数或者时变参数。

S33、考虑到人机协作系统中的安全，在外界接触力矩大于某个阈值时应考虑是否发生碰撞等特殊情况，这时机器人会控制机械臂缓慢回退直到外界接触力矩回到正常范围，可以得到：

其中，k₂为回退速度，τ₃,τ₄为外界接触力矩最大阈值，可以根据不同的操作场景与需求进行调节，可以为常数或者时变参数。

在步骤3得到的关节目标速度指令

基础上，所述步骤4中包含以下1个部分，详述如下：

关节处编码器实时记录关节实际角速度

并每隔一个采样周期T检测机械臂抖动情况，若在采样周期内

发生多次正负变化说明抖动发生，则选取最近一个未发生抖动周期内的关节角速度

作为关节目标速度指令

直到抖动消失。

本发明通过关节检测模块得到的关节位置，速度，力矩，进行动力学参数辨析并构建出动力学模型，通过将动力学模型计算得到的关节力矩与关节力矩传感器反馈的实际力矩计算得到人机接触力，实现了机械臂上任意位置的接触力检测并减小了静摩擦力的干扰。相较于电流检测，关节零力控制效果更好；相较于六维力传感器适用场合更加灵活。采用速度控制而非直接转矩控制，提高了系统的安全性；采用速度控制而非位置控制，提高了系统的响应速度。设计关节抖动补偿模块，解决了现有基于关节力矩反馈的零力控制精度较低，谐振现象容易发生等的不足。

上面结合附图对本发明进行了描述，并不用于限制本发明实施的具体形式，对于本领域的一般技术人员来说，基于本发明进行的形式上和细节上的各种修改，均应包含在本发明的权利要求的保护范围之内。

Claims (5)

1.一种用于坐卧式康复机器人中可抑制抖动的零力控制方法，其特征在于，包括以下步骤：

S1、根据关节检测模块得到的关节位置，速度，力矩构建出动力学模型；

S2、基于动力学模型，关节力矩传感器实时反馈力矩得到外界接触力矩；

S3、根据步骤S2中得到的外界接触力矩计算得到速度指令；

S4、通过关节角度编码器捕捉运动中由于惯性产生的晃动角度，计算得到速度修正量，并补偿到步骤S3中得到的速度指令，消除抖动，实现零力控制。

2.根据权利要求1所述的一种用于坐卧式康复机器人中可抑制抖动的零力控制方法，其特征在于，所述步骤S1包括：

S11、建立机器人动力学模型得到：

其中，M(θ)为惯性项矩阵，

为科氏力及离心力项矩阵，G(θ)为重力项矩阵，F_v为粘摩擦力项矩阵，F_s为静摩擦力项矩阵，

为关节角加速度，

为关节角速度，θ为关节角度，τ为关节力矩，

为与

正负有关的符号函数，τ为关节力矩矩阵；

S12、基于测得的关节力矩，关节角度，角速度，角加速度进行动力学参数辨析，首先对动力学方程进行参数线性化处理得到：

其中，τ为关节力矩矩阵，通过关节力矩传感器获得；

为观测矩阵，P为辨析矩阵，需要通过辨析得到，P＝[p₁,p₂,p₃,…,p_i]，其中p_i＝f(I_i,r_i,l_i,m_i,f_vi,f_si)，其中I_i为连杆i转动惯量，r_i为连杆i质心距离，l_i为连杆i的长度，m_i为连杆i质量，f_vi为连杆i动摩擦力系数,f_si为连杆i静摩擦力系数；

S13、将S12中辨析得到的动力学参数代入S11，得到动力学方程。

3.根据权利要求2所述的一种用于坐卧式康复机器人中可抑制抖动的零力控制方法，其特征在于，所述步骤S2包括：

S21、将关节检测模块得到的实时关节位置θ_t，实时速度

实时加速度

代入S13动力学方程中，得到关节计算力矩τ_t：

S22、根据关节检测模块得到的关节实际反馈力矩τ_e与S21得到的关节计算力矩τ_t计算得到外界接触力矩τ_ext：

τ_ext＝τ_t-τ_e。

4.根据权利要求3所述的一种用于坐卧式康复机器人中可抑制抖动的零力控制方法，其特征在于，所述步骤S3包括：

S31、根据步骤S22中得到的外界接触力矩τ_ext，建立力矩转速转换关系，将其转换为关节目标速度指令

其中，k₁表示力矩速度映射系数，sign(τ_ext)为与τ_ext正负有关的符号函数；

S32、为了避免微小的抖动也被控制系统捕捉，使系统过于敏感，需要设置接触力矩触发阈值，保证系统稳定性，得到：

其中，τ₁,τ₂表示外界接触力矩触发阈值，根据不同的操作场景与需求进行调节，为常数或者时变参数；

S33、考虑到人机协作系统中的安全，在外界接触力矩大于某个阈值时应考虑是否发生碰撞的情况，这时机器人会控制机械臂缓慢回退直到外界接触力矩回到正常范围，得到：

其中，k₂为回退速度，τ₃,τ₄为外界接触力矩最大阈值，根据不同的操作场景与需求进行调节，为常数或者时变参数。

5.根据权利要求4所述的一种用于坐卧式康复机器人中可抑制抖动的零力控制方法，其特征在于：

关节处编码器实时记录关节实际角速度

并每隔一个采样周期T检测机械臂抖动情况，若在采样周期内

发生多次正负变化说明抖动发生，则选取最近一个未发生抖动周期内的关节角速度

作为关节目标速度指令

直到抖动消失。

Images