一种基于干扰观测器的机械系统无力传感触觉力估计方法与流程

1.本发明涉及一种基于干扰观测器的机械系统无力传感触觉力估计 (sensorless haptic force estimation)方法，可用于康复机器人、远程高空作业、 远程医疗手术机器人等，作为触觉传感器估计机械系统末端的外触觉力。


背景技术：

2.自1987年法国医师philippe mouret完成首例腹腔镜胆囊切除手术以来， 微创手术因其创伤小、减轻患者手术疼痛、缩短患者住院时间等优点在越来越 多的传统手术中获得了革命性的成功，包括胆囊切除术、前列腺切除术、阑尾 切除术、子宫切除术、肾切除术、冠状动脉旁路移植术等。然而，微创手术却 在感知和运动方面存在着固有的缺陷，医生不能直接接近操作区域，因此就会 失去直接接触带来的触觉感知，同样的，手术器械与患者组织之间的力觉信息 也会受到限制或产生扭曲。这些都对医生开展组织触摸手术来鉴别组织属性(例 如刚度和弹性模量)和组织异变带来了困难。如前所述，由于难以获得与患者组 织之间的接触信息，医生在执行组织牵拉、分离、缝合等精细手术操作时想要 施加精确的作用力就会变得很困难。最近几年来，有关人机接触交互和微创手 术机器人触觉再现的研究可以为医生提供力觉和触觉反馈，辅助医生执行手术， 增强医生手术灵活性。
3.专利202011528330.x中设计了一种基于振动阵列的触觉刺激系统及方法， 采用振动阵列指令生成装置将原始数据点阵中的信息数据转换成控制指令后发 送给触觉刺激装置；触觉刺激装置的控制器根据控制指令使振动阵列相应的刺 激阵子起振；刺激阵子对原始传达数据轮廓进行展示；振动阵列为单组或多组， 根据原始传达数据和初始化参数自动生成控制指令实时用振动来显示相对应的 轮廓点阵图像，进而与使用者进行信息传递，但是该方法限制了触觉信息再现 的应用场景，由于振动机制本身会带来额外扰动，对于触觉信息测量的精确性 有待商榷，也无法滤除系统本身的外干扰。
4.专利202110030892.x中提出了一种基于超声波相控阵技术的非接触式触 觉反馈系统，该方法将利用超声波换能器阵列向特定区域发射超声波，根据特 定区域的超声波反馈坐标参数，计算各个通道的控制信号，实现超声波换能器 相位的精确控制，实现基于超声波相控阵的非接触式触觉反馈，增强了触觉反 馈的应用范围，减少了接触式触觉固有的摩擦等外部干扰。但该方法局限于近 距离的非接触式触觉信号测量，对于稍远距离的触觉信号则适应性不强，且对 发射超声信号的特定区域环境要求较为苛刻，测量结果不稳定扰动较大。
5.文献1(e.sariyildiz and k.ohnishi,“an adaptive reaction force observerdesign.”ieee transactions on mechatronics,vol.20,pp.750
‑
760,2015.)提出了一 种自适应力反馈观测器，该方法通过自适应的方法自动设计最优力观测器，而 不依赖设计者自身经验，具有更高的力控制带宽，更好的稳定性，并可实现无 外加力传感器的力控制。然而，该算法没有考虑模型中的可建模误差和干扰， 无法将触觉外力从混杂有摩擦等其他低通信号的干扰中分离出来。
6.文献2(z.h.zhao,j.yang,c.j.liu and w.h.chen,“nonlinear compositebilateral control framework for n
‑
dof teleoperation systems with disturbances.
”ꢀ
science china
‑
information sciences,vol.61,pp.1
‑
18,2018.)提出了一种基于双 观测器的遥操作系统触觉再现及控制方法，该方法利用模型干扰观测器和工作 空间干扰观测器，对力和位置信息进行解耦，并对从端机械系统所受外力干扰 进行估计，通过补偿和抑制，实现主端机械系统触觉力的再现。但是该方法具 有触觉力和周期性干扰耦合，无法准确分离，设计较为复杂等缺点。
7.上述现有技术可实现触觉外力的获取和再现，但获取的集总触觉外力信号 中普遍存在周期性低频干扰、高频噪声和建模不确定性等干扰，存在无法得到 精确触觉力的不足，稳定性也较差。


技术实现要素：

8.本发明技术解决问题：针对上述集总触觉外力估计和测量方法中由于存在 周期性低频干扰、高频噪声和建模不确定性等干扰，而无法得到精确触觉力的 不足，提供一种基于鲁棒周期干扰观测器的机械系统无力传感触觉力估计 (sensorless haptic force estimation)方法，提升触觉传感带宽及精度。
9.本发明的技术解决方案为：一种基于干扰观测器的机械系统无力传感触觉 力估计方法，其特征在于针对一类无外接力传感器的机械系统，通过加入鲁棒 周期干扰观测器，实现触觉外力的在线估计，包括以下步骤：
10.第一步，建立基于配备电机位置传感器的机械系统动力学模型，此机械系 统动力学模型中的合外干扰包含已知频率周期振动干扰与低频非周期干扰两部 分，低频非周期干扰中的建模不确定性表示成乘积摄动模型的形式；
11.第二步，基于第一步动力学模型中低频非周期干扰及高频噪声频段特性， 在标准h
∞
控制问题框架下确定优化策略，在连续域中得到鲁棒干扰观测器q
a
(s)， 分离外干扰中的低频慢变外力信号；
12.第三步，基于第一步动力学模型中周期干扰的已知频率特性，在离散域中 设计指定频率陷波器q
p
(z)；
13.第四步，结合第二步中设计的干扰观测器和第三步中的指定频率陷波器， 设计复合鲁棒周期干扰观测器q(z)；
14.第五步，利用所述复合鲁棒周期干扰观测器，建立机械系统触觉力的模型， 实现末端触觉力的估计与分离。
15.所述第一步中，建立基于配备电机位置传感器的机械系统动力学模型的表 达式如下：
[0016][0017]
其中，θ表示电机输出角度，为θ的二阶导数，表示电机输出角加速度， j表示电机惯性矩，τ为力矩，τ
m
和τ
d
分别表示电机总产生力矩和合外干扰力；
[0018]
将总干扰τ
d
离散化，数学形式建模如下：
[0019]
τ
d
(k)＝d
p
(k) d
a
(k)
[0020]
d
p
(k)＝d
p
(k
‑
n) ρ(k)
[0021]
其中，τ
d
(k)为合外干扰，d
p
(k)和d
a
(k)分别为同时存在的周期性振动干扰和 低频非周期干扰，k表示离散时间变量，n表示干扰的周期，ρ(k)为周期干扰的 更新率，模型如下：
[0022][0023]
其中，ρ0(k)为周期干扰的更新函数；
[0024]
将模型不确定性表示为如下乘积摄动形式的集合：
[0025]
ω
j
＝{j
s
(s)＝j
n
(s)
·
[i
△
(s)w(s)]|||
△
(s)||
∞
<1}
[0026]
其中，s为传递函数复变量，j
s
表示真实电机惯量，j
n
表示标称电机惯量， i为单位阵，||
·
||
∞
表示h
∞
范数，
△
(s)表示实际对象频率特性对名义模型的摄动， 满足
△
(s)∈rh
∞
，称为尺度因子，w(s)是归一化权函数，假定
△
(s)和w(s)是稳定的 传递函数，而且
△
(s)的摄动不构成j
n
中不稳定极点的消除，即，j
n
和j
s
具有相同 的不稳定极点，此时
△
(s)是可容许的。
[0027]
所述第二步中，在标准h
∞
控制问题框架下确定优化策略，给出等效开环传 递函数如下：
[0028][0029]
其中，表示虚拟回路，为虚拟被控对象，为虚拟控制器，q
a
(s) 为待设计的鲁棒干扰观测器；为了保证等效传感回路中，q
a
(s)
·
p
n
‑1(s)是物理可实 现的，需满足：
[0030]
q
a
(s)∈ω
μ
，ω
μ
＝{f(s)|f(s)＝n(s)/d(s),deg(d(s))
‑
deg(n(s))≥μ}
[0031]
其中，ω
μ
表示相对阶次为μ的有理分式集合，μ表示原系统标称对象的相对 阶次，f(s)表示集合中的元素，n(s)和d(s)分别表示有理分式的分子多项式和分 母多项式，deg(
·
)表示多项式的阶次，即，观测器q
a
(s)的相对阶次需大于等于μ， 则虚拟回路满足：
[0032]
当且仅当q
a
(s)∈ω
μ
[0033]
基于标准h
∞
控制问题框架，采用如下混合灵敏度设计规则：
[0034][0035]
其中，γ为调节参数。w
t
(s)、w
u
(s)和w
s
(s)分别为反映噪声与模型不确定 性、控制输入、低频非周期外干扰力三通道频谱特性的特征权函数；
[0036]
至此，将鲁棒干扰观测器的设计问题转化为利用广义虚拟回路整形方法的 标准h
∞
问题，用matlab软件进行计算；
[0037]
虚拟被控对象与回路整形所得虚拟控制器均不唯一，但回路 唯一，则得到最优的鲁棒干扰观测器q
a
(s)，表示如下：
[0038][0039]
所述第三步中，根据合外干扰中的周期干扰已知频率，在离散域中设计指 定频率陷波器如下：
[0040]
q
p
(z)＝[1
‑
λ(1
‑
z
‑
n
)]
[0041]
其中，q
p
(z)为指定频率陷波器，λ为设计参数，z为离散传递函数复变量， 选取λ＝0.5使得下式最小：
[0042]
|q
p
(z)|＝|1
‑
2λ|
[0043]
其中，|
·
|表示向量的2
‑
范数。
[0044]
所述第四步中，设计复合鲁棒周期干扰观测器如下：
[0045]
q(z)＝q
a
(s)
·
q
p
(z)＝q
a
(z)[1
‑
λ(1
‑
z
‑
n
)]。
[0046]
所述第五步中，给出机械系统触觉力模型如下：
[0047]
τ
d
＝τ
int
τ
f
τ
hap
τ
l
τ
g
[0048]
其中，τ
d
表示末端所受合外干扰力，τ
int
表示相互作用力，包括哥式力和离 心力；τ
f
表示摩擦力，包括库仑摩擦力和粘性摩擦力；τ
hap
、τ
l
和τ
g
分别表示触 觉外力、负载力和重力；
[0049]
则触觉外力估计表示为：
[0050][0051]
其中，表示触觉外力估计，表示合外力估计，τ
l
和τ
g
是恒定且可预知 补偿的，机械系统末端所受摩擦力τ
f
和内部交互力τ
int
是可通过重复环境实验预 先辨识的；
[0052]
利用所得复合鲁棒周期干扰观测器q(z)＝q
a
(z)[1
‑
γ(1
‑
z
‑
n
)]，得到合外干扰 力估计如下：
[0053][0054][0055]
其中，和分别表示合外干扰力τ
d
和电机控制输入τ
m
到合外干扰力估 计的传递函数，p
s
(s)为实际系统的数学模型，p
n
(s)表示标称系统的数学模型；
[0056]
进而得到机械系统末端所受触觉力估计：
[0057][0058]
基于以上方法过程，在保障对机械系统模型不确定性的抑制基础上，实现 对机械系统末端测量噪声、周期性振动等干扰信号的有效过滤，提高触觉力估 计的准确性。
[0059]
本发明与现有技术相比的优点：本发明通过加入复合鲁棒周期干扰观测 器，实现触觉力的在线估计，而无需外接力传感器。相比于传统的外接力传 感器，本发明方法的可
靠性不受外设传感器加工工艺的影响，可保证系统的 鲁棒稳定，且过滤高频噪声和周期性振动干扰，提升触觉力传感的带宽及精 度。本发明可用于康复机器人、远程高空作业、远程医疗手术机器人等。
附图说明
[0060]
图1为本发明方法实现流程图；
[0061]
图2为本发明中合外干扰估计系统框图。
具体实施方式
[0062]
下面结合附图及实施例对本发明进行详细说明。
[0063]
如图1所示，本发明具体实现如下：
[0064]
第一步，建立机械系统动力学模型，并对合外干扰与模型不确定性进行建 模分析；
[0065]
建立如下配备电机位置传感器的机械系统动力学模型：
[0066][0067]
其中，θ表示电机输出角度，为θ的二阶导数，表示电机输出角加速度， j表示电机惯性矩，τ为力矩，τ
m
和τ
d
分别表示电机总产生力矩和合外干扰力。
[0068]
将合外干扰力τ
d
离散化，数学形式建模如下：
[0069]
τ
d
(k)＝d
p
(k) d
a
(k)
[0070]
d
p
(k)＝d
p
(k
‑
n) ρ(k)
[0071]
其中，τ
d
(k)为合外干扰力，d
p
(k)和d
a
(k)分别为同时存在的周期性振动干扰 和低频非周期干扰。k表示离散时间变量，n表示干扰的周期，ρ(k)为周期干扰 的更新率，模型如下：
[0072][0073]
其中，ρ0(k)为周期干扰的更新函数。
[0074]
考虑将模型不确定性表示为如下乘积摄动形式的集合：
[0075]
ω
j
＝{j
s
(s)＝j
n
(s)
·
[i δ(s)w(s)]|||δ(s)||
∞
<1}
[0076]
其中，s为传递函数复变量，j
s
表示真实电机惯量，j
n
表示标称电机惯量， i为单位阵，||
·
||
∞
表示h
∞
范数。δ(s)表示实际对象频率特性对名义模型的摄动， 满足δ(s)∈rh
∞
，称为尺度因子。w(s)是归一化权函数，通常假定δ(s)和w(s)是稳 定的传递函数，而且δ(s)的摄动不构成j
n
中不稳定极点的消除，即，j
n
和j
s
具有 相同的不稳定极点，此时δ(s)是可容许的。
[0077]
第二步，根据干扰中的非周期部分及高频噪声频段特性，将传感估计系统 转化为标准h
∞
控制问题，在连续域标准h
∞
控制问题框架下给定优化策略，设计 鲁棒干扰观测器q
a
(s)，分离外干扰中的低频慢变信号；
[0078]
根据等效鲁棒非周期干扰估计补偿系统结构，给出系统输入输出传递函数：
[0079]
从控制输入τ
m
到输出θ的传递函数：
[0080][0081]
从干扰输入τ
d
到输出θ的传递函数：
[0082][0083]
从噪声输入η到输出θ的传递函数：
[0084][0085]
给出等效回路开环传递函数如下：
[0086][0087]
其中，表示虚拟回路，为虚拟被控对象，为虚拟控制器。为了 保证等效传感回路中，q
a
(s)
·
p
n
‑1(s)是物理可实现的，需满足：
[0088]
q
a
(s)∈ω
μ
，ω
μ
＝{f(s)|f(s)＝n(s)/d(s),deg(d(s))
‑
deg(n(s))≥μ}
[0089]
其中，ω
μ
表示相对阶次为μ的有理分式集合，μ表示原系统标称对象p
n
(s)的 相对阶次，f(s)表示集合中的元素，n(s)和d(s)分别表示有理分式的分子多项式 和分母多项式，deg(
·
)表示多项式的阶次。即，观测器q
a
(s)的相对阶次需大于等 于μ，则虚拟回路满足：
[0090]
当且仅当q
a
(s)∈ω
μ
[0091]
则虚拟回路的灵敏度函数s(s)及补灵敏度t(s)为：
[0092][0093]
采用如下混合灵敏度设计规则：
[0094][0095]
其中，γ为调节参数；w
t
(s)、w
u
(s)和w
s
(s)分别为反映噪声与模型不确定 性、控制输入和输入外干扰力三通道频谱特性的特征权函数。将虚拟回路的灵 敏度与补灵敏度函数带入设计规则：
[0096]
[0097]
至此，将鲁棒干扰观测器的设计问题转化为利用广义虚拟回路整形方法的 标准h
∞
问题。可用matlab软件进行计算。
[0098]
在实际进行权函数设计时，w
u
(s)选为常值。w
t
(s)需要在高频段反映系统 不确定性的上界及位移通道观测噪声的频谱特性，且在中频段反映系统的动态 性能。w
s
(s)应反映低频输入外力的频谱特性，通常将低频非周期干扰的传递函 数视为q阶次，则w
s
(s)应满足内模阶次特性，即，存在充分小参数ε>0，使得 w
s
(s)包含q 1个重复极点s＝
‑
ε。
[0099]
的选择需满足：在右半平面解析且无零点，在右半平面解析且无零点，
[0100]
经此方法，虚拟被控对象与回路整形所得虚拟控制器均不唯一，但 回路唯一，则鲁棒干扰观测器q
a
(s)唯一，表示如下：
[0101][0102]
第三步，根据干扰中的已知频率周期干扰频律特性，在离散域中设计指定 频率陷波器，结合第二步中设计的鲁棒干扰观测器，设计复合鲁棒周期干扰观 测器。
[0103]
根据周期干扰的已知频率，在离散域中设计指定频率陷波器如下：
[0104]
q
p
(z)＝[1
‑
λ(1
‑
z
‑
n
)]
[0105]
其中，q
p
(z)为指定频率陷波器，λ为设计参数，z为离散传递函数复变量， 选取λ＝0.5使得下式最小：
[0106]
|q
p
(z)|＝|1
‑
2λ|
[0107]
其中，|
·
|表示向量的2
‑
范数。
[0108]
将第二步中设计的鲁棒干扰观测器q
a
(s)离散化为q
a
(z)。
[0109]
设计复合鲁棒周期干扰观测器如下：
[0110]
q(z)＝q
a
(z)
·
q
p
(z)＝q
a
(z)[1
‑
λ(1
‑
z
‑
n
)]
[0111]
第四步，对机械系统末端进行受力情况分析，给出机械系统低频合外力干 扰的模型如下：
[0112]
τ
d
＝τ
int
τ
f
τ
hap
τ
l
τ
g
[0113]
其中，τ
int
表示相互作用力，包括哥式力和离心力；τ
f
表示摩擦力，包括库 仑摩擦力和粘性摩擦力；τ
hap
、τ
l
和τ
g
分别表示触觉外力、负载力和重力。
[0114]
则触觉外力估计可表示为：
[0115][0116]
其中，表示触觉外力估计，表示合外力估计，τ
l
和τ
g
是恒定且可预知 补偿的，机械系统末端所受摩擦力τ
f
和内部交互力τ
int
是可通过重复环境实验预 先辨识的。
[0117]
利用前述三步中所得复合鲁棒周期干扰观测器q(z)＝q
a
(z)[1
‑
γ(1
‑
z
‑
n
)]，得 到低频合外干扰力估计如下：
[0118][0119]
其中，和分别表示合外干扰力τ
d
和电机控制输入τ
m
到合外干扰力估 计的传递函数。
[0120]
进而，可得到机械系统末端所受触觉力估计：
[0121][0122]
其中，表示触觉力估计。
[0123]
基于以上过程，可在保障对机械系统模型不确定性的抑制基础上，实现对 机械系统测量噪声、周期性振动等干扰信号的有效过滤，提高触觉力估计的准 确性。
[0124]
本发明说明书中未作详细描述内容属于本领域专业技术人员公知现有技术。