融合姿态测量与一维力传感器信息的模糊变阻抗控制方法与流程

1.本发明涉及机器人运动控制技术领域，更具体的说是一种融合姿态测量与一维力传感器信息的模糊变阻抗控制方法。


背景技术：

2.近年来康养理疗领域的冲击波理疗机器人、按摩机器人等得到快速发展，机器人工作时主要从法向的方向以垂直的方式对人体进行理疗按摩。理疗、按摩机器人需要与人体进行物理接触。因此，为了实现理疗按摩的安全性，需要对理疗按摩方向进行交互力控制。
3.为了实现安全的交互力控制，常采用的力/力矩传感器有两种方式：一是采用关节力矩传感器，即每个关节安装一个力矩传感器；二是采用末端六维力/力矩传感器。两种方式虽然各有利弊，但有一个共同的缺点就是成本昂贵。因此，开发低成本的力控是本发明需要解决的问题。


技术实现要素：

4.针对现有交互力控制方法成本昂贵的不足，本发明的主要目的在于提供一种融合姿态测量与一维力传感器信息的模糊变阻抗方法，以提供低成本、高精度的交互力控制方案，为冲击波理疗机器人、按摩机器人提供安全交互力控制方法。
5.为实现上述目的，本发明提供了如下技术方案：融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，包括步骤s1：通过第一传感器检测机器人末端到目标表面距离，以获得目标表面位置信息；
6.步骤s2：根据目标表面位置信息构建目标表面曲面模型；
7.步骤s3：根据曲面模型求得轨迹点处的曲面法向量
8.步骤s4：通过曲面法向量调整一维力传感器测量方向与曲面的法向量重合；
9.步骤s5：通过一维力传感器测量目标曲面法向的接触力f信息；
10.步骤s6：控制器接收第一传感器和一维力传感器信号并在x，y方向控制机器人运动轨迹，z方向控制接触力f。
11.作为本发明的进一步改进，步骤s1中第一传感器由多个tof激光距离传感器组成。
12.作为本发明的进一步改进，步骤s2中目标表面位置信息使用最小二乘法或者曲面拟合法构建目标表面曲面模型。
13.作为本发明的进一步改进，步骤s3中曲面法向量的求得包括以下步骤：
14.步骤s31、对曲面模型方程各变量求偏导；
15.步骤s32、代入δh轨迹点处数据求得δh轨迹点处曲面沿x和y方向曲面切向量；
16.步骤s33、根据曲面切向量求得δh轨迹点处的曲面法向量。
17.作为本发明的进一步改进，步骤s6中的控制器包括正运动学回路和逆运动学回路，所述一维力传感器设置在正运动学回路内，所述逆运动学回路内设有算法模块并与一
维力传感器连接，所述算法模块用于接收一维力传感器信号并给出执行端需求变量。
18.作为本发明的进一步改进，所述算法模块定义目标阻抗为
[0019][0020]
其中f
ext
为机械臂末端产生相应的外力，m、b、k分别为位置控制的质量、阻尼系数和刚度系数，e
x
、分别为阻抗控制作用下机械臂末端的运动位置偏差、速度偏差与加速度偏差。
[0021]
作为本发明的进一步改进，所述逆运动学回路内设有模糊变阻抗参数调节模块，所述模糊变阻抗参数调节模块与算法模块连接并用于调整阻尼系数b。
[0022]
作为本发明的进一步改进，所述阻尼系数基于模糊规则进行设计。
[0023]
作为本发明的进一步改进，所述逆运动学回路内还设有滑模控制器，所述滑模控制器分别连接算法模块和一维力传感器，所述滑模控制器用于为算法模块同滑模控制。
[0024]
作为本发明的进一步改进，所述滑模模块对应趋近律为双曲切函数。
[0025]
本发明的有益效果：
[0026]
1、通过传感器获得目标表面数据，并通过该数据进行目标曲面模型建立，进而实现一维力传感器测量方向始终与曲面法向量重合，以保证机器人相较于目标表面的垂直。
[0027]
2、通过一维力传感器和第一传感器的设置配合控制器实现对机器人方位的调整，并且通过正运动学回路和逆运动学回路的设置保证对运动偏差值的修正；
[0028]
3、仅通过一维力传感器和tof激光测距传感器完成检测感应效果，简单可行，成本低。
附图说明
[0029]
图1为本发明整体流程图；
[0030]
图2为本发明多个tof激光距离传感器排布方式示意图；
[0031]
图3为本发明控制器控制流程图；
[0032]
图4为本发明机器人动力学的等效模型图；
[0033]
图5为本发明阻尼参数输出图。
具体实施方式
[0034]
下面结合附图和实施例，对本发明进一步详细说明。其中相同的零部件用相同的附图标记表示。
[0035]
参照图1，融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，包括步骤s1：通过第一传感器检测机器人末端到目标表面距离，以获得目标表面位置信息；
[0036]
优选的，步骤s1中第一传感器由多个tof激光距离传感器组成，进一步优选，为了减少或消除测量盲区，参照图2，多个tof激光距离传感器呈圆周分布设置。
[0037]
步骤s2：根据目标表面位置信息构建目标表面曲面模型；
[0038]
优选的，步骤s2中目标表面位置信息使用最小二乘法或者曲面拟合法构建目标表面曲面模型，在本实施例中，通过tof激光距离传感器获取测量的位置信息，每一个tof传感
器测量获取一个曲面，根据位置信息及tof传感器当前视角计算视角内实际物体曲面模型。
[0039]
步骤s2在具体使用时，根据tof传感器分布位置，分别计算获取的位置信息，组成测量范围内的位置数据集(x1,y1,z1)，(x2,y2,z2)
…
(xn,yn,zn)，将数据集(x1,y1,z1)至(xn,yn,zn)之间重叠的点位采用最小二乘法进行拟合，将曲面进行拼接，实现整个测量区域内完整曲面模型f(x,y,z)＝0的构建。
[0040]
步骤s3：根据曲面模型求得轨迹点处的曲面法向量
[0041]
优选的，步骤s3中曲面法向量的求得包括以下步骤：
[0042]
步骤s31、对曲面模型方程f(x,y,z)＝0各变量求偏导；
[0043]
步骤s32、代入δh轨迹点处数据求得δh轨迹点处曲面沿x和y方向曲面切向量；
[0044]
步骤s33、根据曲面切向量求得轨迹点δh处的曲面法向量
[0045]
其中，当机器人作用于目标表面，到达设定的最大接触力阈值时，也认为此时是目标最大变形处，通过机器人运动学可以计算得到该处机器人位置f0(x0,y0,z0)，此时最大变形量δh＝|fn(xn,yn,zn)-f0(x0,y0,z0)|(f0(x0,y0,z0)为机器人刚接触目标表面，接触力为零时的位置)，并计算变形最大δh处的法向量
[0046]
步骤s4：通过曲面法向量利用机器人逆运动学调整机械臂姿态使一维力传感器测量方向与曲面的法向量重合，进而使得机器人从法向方向与目标接触；
[0047]
步骤s5：通过一维力传感器测量目标曲面法向的接触力f信息；
[0048]
步骤s6：控制器接收第一传感器和一维力传感器信号并在x，y方向控制机器人运动轨迹，z方向控制接触力f，其中，z方向为垂直目标表面的方向，x，y方向与z方向垂直，进而实现机器人在不同约束方向上执行不同的动作。
[0049]
参照图1和图3，步骤s6中的控制器包括正运动学回路和逆运动学回路，一维力传感器设置在正运动学回路内，逆运动学回路内设有算法模块并与一维力传感器连接，算法模块用于接收一维力传感器信号并给出执行端需求变量。
[0050]
优选的，算法模块定义目标阻抗为
[0051][0052]fext
为机械臂末端产生相应的外力，m、b、k分别为位置控制的质量、阻尼系数和刚度系数，e
x
、分别为阻抗控制作用下机械臂末端的运动位置偏差、速度偏差与加速度偏差。
[0053]
参照图3，为了实现机器人z方向上接触力f的控制，进而实现末端柔顺性控制的目，逆运动学回路内设有模糊变阻抗参数调节模块，模糊变阻抗参数调节模块与算法模块连接并用于调整算法模块阻尼系数b。
[0054]
参照图4，当机械臂末端位置x和理想轨迹xc产生偏差e
x
＝x
c-x,时，机械臂末端产生相应的外力f
ext
，此时将公式(1)转换成传递函数形式，并与二阶系统传递函数进行对比，可得：
[0055][0056]
则刚度系数k为：
[0057][0058]
ξ为系统的阻尼比，在式(2)中，s是传递函数是原函数经过拉普拉斯变换后的表达。考虑到系统的稳定性和可靠性，机械臂与外界接触时机械臂末端运动应保持平稳而无超调，因此二阶系统表现出过阻尼的特性，即ξ≥1，可将阻尼比ξ设定为1.2。由式(2)可知，对于系统阻抗参数的选择，在保证系统稳定无超调的前提下，通过实时调节阻尼参数b，则刚度矩阵k可通过式(3)获得，从而实现末端柔顺性控制的目的。
[0059]
参照图5，为了使机械臂与外界接触过程中表现的更为柔顺，本发明的阻尼系数b基于模糊规则进行设计，通过机械臂与外界交互过程中加速或减速趋势判断的基础上，将上一时刻机械臂末端的运动速度v和人的接触力f作为模糊控制器的输入变量，其范围分别为(-0.15，0.15)m/s和(-6，6)n，输出为阻抗参数b，范围定义为(10，20)n/(m/s)。根据模糊推理系统(fis)，输入和输出变量的语言值定义为：{nb、ns、z、ps、pb}、{s、sm、m、bm、b}。因此输入和输出变量可以用五个三角形的成员隶属度函数来表示，最终得到的阻尼参数输出结果如图5所示。
[0060]
优选的，逆运动学回路内还设有滑模控制器，滑模控制器分别连接算法模块和一维力传感器，滑模控制器用于为算法模块同滑模控制。
[0061]
本发明滑模控制策略如下：根据式(1)可知，当接触外力f
ext
为零时，则末端位置误差e
x
也应该为零。因此式(1)可以展开为：
[0062][0063]
xd(0)＝xc(0)；
[0064]
其中xd，分别为机械臂末端的期望位置、期望速度和期望加速度；xc，分别为机械臂末端的理想位置、理想速度和理想加速度。
[0065]
当机械臂的末端轨迹未达到期望的轨迹跟踪时，这意味着微小的位置偏差可能产生较大的力/力矩。结合接触外力作用下的笛卡尔坐标系下的动力学方程：定义滑模面s及趋近律如下：
[0066]
e(t)＝xd(t)-x(t)
ꢀꢀ
(5)
[0067][0068][0069][0070]
其中e(t)、s(t)、分别是位置跟踪误差、滑模面和趋近律，c为正数，决定滑模面收敛速度。为机械臂实际速度，λ和η均大于零，ε》0。
[0071]
优选的，滑模模块对应趋近律为双曲切函数(式(8))，以减少滑模面切换时的抖振现象。
[0072]
将式(5)代入式(7)中，可以得到：
[0073]
[0074]
依据和式(9)，可以推导出：
[0075][0076]
其中，c
x
为机器人动力学方程中的离心力与哥氏力项，接着将式(6)和式(8)代入式(10)，则系统控制律为：
[0077][0078]
为了证明控制器的稳定性，选择李雅普诺夫函数为：
[0079][0080]
将式(12)对时间求导得到：
[0081][0082]
惯性矩阵m
x
是正定矩阵，是斜对称矩阵。将滑模控制器的闭环控制系统代入式(13)中，可得：
[0083][0084]
利用不等性性质，结合式(13)和(14)，可以得到：
[0085][0086]
其中λ
max
(m
x
),λ
min
(k)分别为m
x
，k的最大和最小特征值，b＝ημε。
[0087]
利用性质对于f,v∈[0,+∞)∈r,则则可以化简为：
[0088][0089]
将式(16)带入式(15)，得到：
[0090][0091]
综上，可以通过选择参数c来保证滑模面s的稳定性，由式(17)可知为负定矩阵，当s＝0时系统全局渐近稳定。
[0092]
因此，通过融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，用于机器人运动控制技术领域，包括冲击波机器人、按摩机器人等适用本方案的机器人技术领域，是
一种稳定、简单可行、成本低的解决方案。
[0093]
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

技术特征：
1.融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：包括步骤s1：通过第一传感器检测机器人末端到目标表面距离，以获得目标表面位置信息；步骤s2：根据目标表面位置信息构建目标表面曲面模型；步骤s3：根据曲面模型求得轨迹点处的曲面法向量步骤s4：通过曲面法向量调整一维力传感器测量方向与曲面的法向量重合；步骤s5：通过一维力传感器测量目标曲面法向的接触力f信息；步骤s6：控制器接收第一传感器和一维力传感器信号并在x，y方向控制机器人运动轨迹，z方向控制接触力f。2.根据权利要求1所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：步骤s1中第一传感器由多个tof激光距离传感器组成。3.根据权利要求2所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：步骤s2中目标表面位置信息使用最小二乘法或者曲面拟合法构建目标表面曲面模型。4.根据权利要求1所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：步骤s3中曲面法向量的求得包括以下步骤：步骤s31、对曲面模型方程各变量求偏导；步骤s32、代入δh轨迹点处数据求得δh轨迹点处曲面沿x和y方向曲面切向量；步骤s33、根据曲面切向量求得轨迹点δh处的曲面法向量5.根据权利要求1所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：步骤s6中的控制器包括正运动学回路和逆运动学回路，所述一维力传感器设置在正运动学回路内，所述逆运动学回路内设有算法模块并与一维力传感器连接，所述算法模块用于接收一维力传感器信号并给出执行端需求变量。6.根据权利要求5所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：所述算法模块定义目标阻抗为其中f
ext
为机械臂末端产生相应的外力，m、b、k分别为位置控制的质量、阻尼系数和刚度系数，e
x
、分别为阻抗控制作用下机械臂末端的运动位置偏差、速度偏差与加速度偏差。7.根据权利要求6所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：所述逆运动学回路内设有模糊变阻抗参数调节模块，所述模糊变阻抗参数调节模块与算法模块连接并用于调整阻尼系数b。8.根据权利要求6所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：所述阻尼系数基于模糊规则进行设计。9.根据权利要求5所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，其特征在于：所述逆运动学回路内还设有滑模控制器，所述滑模控制器分别连接算法模块和一维力传感器，所述滑模控制器用于为算法模块同滑模控制。10.根据权利要求9所述的融合姿态测量与一维力传感器信息的模糊变阻抗控制方法，
其特征在于：所述滑模模块对应趋近律为双曲切函数。

技术总结
本发明公开了融合姿态测量与一维力传感器信息的模糊变阻抗控制方法包括步骤S1：通过第一传感器检测机器人末端到目标表面距离，获得目标表面位置信息，步骤S2：根据目标表面位置信息构建目标表面曲面模型，步骤S3：根据曲面模型求得轨迹点处的曲面法向量，步骤S4：通过曲面法向量调整一维力传感器测量方向与曲面的法向量重合，步骤S5：通过一维力传感器测量目标曲面法向的接触力信息，步骤S6：控制器接收第一传感器和一维力传感器信号并在X，Y方向控制机器人运动轨迹，Z方向控制接触力，是一种稳定、简单可行、成本低的解决方案。成本低的解决方案。成本低的解决方案。


技术研发人员：李银鑫 范鹏灿 禹超 安轲 谈全一 张斌 朵天泽 吴方勇
受保护的技术使用者：浙江钧控智能科技有限公司
技术研发日：2023.05.09
技术公布日：2023/8/4