一种基于数据驱动模型的软镜扭转运动安全控制方法

1.本发明属于医疗机器人控制技术领域，具体地涉及一种基于数据驱动模型的软镜扭转运动安全控制方法。


背景技术：

2.基于软镜操作技术实现的经人体自然腔道内镜手术，作为一种典型的微创甚至无创手术在最近几年受到了广泛的关注并应用于多种疾病诊查与外科手术中。对于软镜的扭转运动而言，由于引导鞘内存在不连续的复杂摩擦力，会使得软镜手柄端和软镜末端的扭转状态不一致，导致软镜镜体在引导鞘内的扭转形变累加、扭力增加，当镜体形变产生的扭力大于静摩擦力时，软镜末端扭转角才会随手柄扭转而运动。
3.当前对软镜扭转运动的研究，通过对软镜扭转运动的真实运动机理进行分析，使用具有相似动态特性的机构与之拟合，建立可解释性强的物理模型，在模型中添加不确定性因素，但是机理模型多维结构和复杂的数学表达式难以设计基于模型的控制器，并且无法实时补偿模型误差。使用数据驱动模型可以高精度描述动力系统的非线性特性，但实际应用中基于有限维的koopman算子的数据驱动模型存在明显的模型误差，降低了软镜扭转运动控制的安全性。


技术实现要素：

4.本发明为克服现有技术的不足，提供一种基于数据驱动模型的软镜扭转运动安全控制方法。该方法克服了有限维koopman算子的数据驱动模型存在的模型误差缺陷产生的抖振和失稳对软镜末端扭转安全的影响。
5.一种基于数据驱动模型的软镜扭转运动安全控制方法包含：
6.步骤一、基于koopman算子原理，设计得到软镜扭转迟滞系统驱动模型：
7.g(β
k 1
,τ
k 1
)＝k
t
g(βk,τk) εk8.其中，a＝k
t
∈rn×n表示有限维的koopman矩阵，g(βk,τk)和g(β
k 1
,τ
k 1
)为一个固定的观测函数，εk∈r9×1表示软镜扭转迟滞系统驱动模型的有限维近似误差；
9.步骤二、设计控制器，即设计基于时滞估计的控制律
10.对于软镜扭转迟滞系统驱动模型而言，控制目标是操作软镜的末端扭转角度跟踪期望的目标轨迹βd，并使得跟踪误差能够收敛到零，输出不应超过软镜操作时的安全约束，即满足β(t)∈(0,ξ),设计控制律为：
[0011][0012][0013]
其中，τ是软镜扭转迟滞系统的输入量，ξ是预设的安全边界，β表示软镜末端扭转角度，βd是软镜末端跟踪期望的目标轨迹，e是表示跟踪误差，k
p
表示控制器的控制增益，sgn
是符号函数，z是始终为正的辅助信号，||η(d,t)||表示可行区间的边界；
[0014][0015]
该边界的数值通过集元滤波器模型求得。
[0016]
本发明相比现有技术的有益效果是：
[0017]
1、本发明所提控制策略具有良好的跟踪控制性能和快速的瞬态响应，且在软镜扭转迟滞系统驱动模型边界约束安全性强化的同时，提高了系统的鲁棒性。
[0018]
2、本发明克服了有限维koopman算子的数据驱动模型存在的模型误差缺陷产生的抖振和失稳对软镜末端扭转安全的影响。
[0019]
下面结合附图和实施例对本发明的技术方案作进一步地说明：
附图说明
[0020]
图1为本发明结合的软镜操作机器人姿态状态图；
[0021]
图2为本发明结合的软镜操作机器人的刚性关节和软镜末端的相互作用关系图；
[0022]
图3为本发明软镜扭转迟滞系统驱动模型的输出值与扭转角度的真实测量值在不同输入区间内变频输入信号下的多重对比结果图；
[0023]
图4为动态数据驱动模型的估计值，koopman算子模型的输出值和扭转角度的真实测量值在不同输入区间内变频输入信号下的多重对比结果图；
[0024]
图5为实施例中第一种实验下的本技术控制方法、对比emc方法和对比smc方法下的软镜末端扭转角、跟踪误差和输入量的关系图；
[0025]
图6为实施例中第二种实验下的本技术控制方法、对比emc方法和对比smc方法下的软镜末端扭转角、跟踪误差和输入量的关系图；
[0026]
图7为实施例中第三种实验下的本技术控制方法、对比emc方法和对比smc方法下的软镜末端扭转角、跟踪误差和输入量的关系图。
具体实施方式
[0027]
下面将结合附图对本发明技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本发明的技术方案，因此只作为示例，而不能以此来限制本发明的保护范围。
[0028]
需要注意的是，除非另有说明，本技术使用的技术术语或者科学术语应当为本发明所属领域技术人员所理解的通常意义。
[0029]
为了更清晰地说明本发明的实现过程，现将本发明的具体实施方式介绍如下。
[0030]
如图1-图2所示，本实施方式的一种基于数据驱动模型的软镜扭转运动安全控制方法，包含以下步骤：为设计驱动模型，
[0031]
首先，设计改进的集元滤波器模型：
[0032]
将动力学系统使用koopman算子表示：
[0033][0034]
其中g(xk,τk)∈rn表示所设计的候选观测函数g(
·
)在k时刻的取值，a＝k
t
∈rn×n表示有限维近似的koopman矩阵，xk,τk∈rr分别表示系统在k时刻的状态量和输入量，z
k 1
∈rm表示该系统在k 1时刻的观测值，c是一个参数矩阵，而vk∈rn,wk∈rm分别表示koopman算子的有限维近似误差和测量误差；
[0035]
不失一般性，这里假设所有的系统噪声都是未知但有界的，且可用可行集来表示误差的界限，即表示为vk∈e(0,qk),wk∈e(0,rk)，qk和rk表示误差界限，包络椭圆e(
·
,
·
)的定义如下：
[0036]
e(c,l)＝{x∈rn|(x-c)l-1
(x-c)≤1}
ꢀꢀ
(2)
[0037]
其中c表示椭圆可行集的中心状态，包络矩阵l为正定对称矩阵并决定了对应椭圆可行集e(c,l)的大小，于是，基于观测函数g(
·
)的集元滤波算法可写为：
[0038][0039]
其中，表示不同时刻的集元滤波可行集椭球形状矩阵，η表示椭球形状的参数变量；
[0040]
而更新过程为：
[0041][0042]
其中，ρ表示椭圆形状的参数变量，gk表示更新增益，wk是推导中间变量，δk为表示奇异程度的递归计算指数，迭代计算表达式如下：
[0043][0044]
由此得到数据驱动的集元滤波器模型；
[0045]
然后，设计一个固定的观测函数g(βk,τk)，定义g(βk,τk)的具体表达形式为：
[0046][0047]
其中τk表示软镜扭转运动时手柄在k时刻的扭转角度，βk为软镜末端在k时刻的扭转角度，这里的观测函数g(βk,τk)的选取方法兼顾了系统的时变动态和特殊非线性，并在其中加入了状态信息和控制量的耦合项，将任务的几何信息与系统状态相关的响应信息结合起来，接下来，通过引入koopman算子，得到软镜扭转迟滞系统驱动模型：
[0048]
g(β
k 1
,τ
k 1
)＝k
t
g(βk,τk) εkꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0049]
进一步地，软镜扭转迟滞系统驱动模型的有限维近似误差估算：
[0050]
为了方便构造动态模型，首先将公式(7)整理得到；
[0051][0052]
其中，gk为扩展离散状态向量，y
k 1
∈r9×1是系统的测量向量
[0053][0054]
对应地，离散系统的参数矩阵kd和cd为：
[0055][0056][0057]
一般地，系统的输入量τk认为是准确可测的，且未知系统误差vk，ωk是有界的。于是，进一步假设vk∈e(0,qk),wk∈e(0,rk)来表示软镜扭转迟滞系统驱动模型的误差εk和系统中其他混合噪声。
[0058]
基于软镜扭转迟滞系统驱动模型的集元滤波算法实时估计出误差εk的预测值和置信区间，定义符号η(εk)为软镜扭转迟滞系统驱动模型的有限维近似误差εk在k时刻的估计区间，推导得到：
[0059][0060]
其中，是对所述误差εk的估计值，表示误差向量的第i个元素，表示矩阵的第(9 i,9 i)个元素。
[0061]
下一步，设计控制器，基于数据采样技术，利用集元滤波估计误差区间的优势，设计一种鲁棒控制方法：对已辨识固定的软镜扭转迟滞系统驱动模型，推导解耦形式，首先将观测函数向量分解为：
[0062]
g(βk,τk)＝g1(βk) g2(βk)τkꢀꢀꢀꢀꢀꢀ
(13)
[0063][0064]
将软镜扭转迟滞系统模型作如下表达：
[0065][0066]
其中，c∈r9×9为单位矩阵，y
k 1
∈r9×1是系统的测量向量，公式(15)可基于数据采样技术表示成如下形式：
[0067][0068]
[0069]
上式中，δt表示系统的采样周期，k
t
(1,:)表示k
t
的第1行向量，同理ε(t)(1)表示ε(t)的第一个元素，τ(t),β(t),ε(t)分别表示τk,βk,εk在时间t∈[kδt,(k 1)δt]内的连续形式，k为非负整数，即有：
[0070][0071]
于是，模型误差的边界η(d,t)可以重新写作如下形式：
[0072][0073]
其中表示在时刻t∈[kδt,(k 1)δt]时的连续形式；
[0074]
对于软镜扭转运动系统驱动模型而言，控制目标是操作软镜的末端扭转角度跟踪期望的目标轨迹βd，并使得跟踪误差能够收敛到零，实施应用中，输出不应超过软镜操作时的安全约束，即满足β(t)∈(0,ξ),ξ是预设的安全边界，利用软镜扭转运动系统驱动模型的有限维近似误差的估计值，再结合模型误差可行区间的实时估计结果，设计控制律为：
[0075][0076]
该安全强化的跟踪控制器进一步补偿系统中的迟滞效应，也可以减少未初始化工作空间对参考数据驱动模型精确度的影响，
[0077]
其中，τ是软镜扭转迟滞系统的输入量，ξ是预设的安全边界，β表示软镜末端扭转角度，βd是软镜末端跟踪期望的目标轨迹，e是表示跟踪误差，k
p
表示控制器的控制增益，sgn是符号函数，z是始终为正的辅助信号，||η(d,t)||表示公式(19)所述可行区间的边界。
[0078]
进一步地，为了证明本实施方式所提的控制器的闭环系统对于目标轨迹是指数稳定的，进行了稳定性证明：
[0079]
首先选取候选lyapunov函数为：
[0080][0081]viblf
是积分障碍lyapunov函数：
[0082][0083]
由于z始终为正，在软镜运动的受限区域内可保证通过v
iblf
≥0
[0084]
对v求导，可以得到:
[0085][0086]
其中λ＝min{2k
p
,2}，且可以推导出v(t)是指数收敛到0的。相应地，在整个控制过程中始终满足于是可推断
[0087][0088]
这里考虑系统输出约束β∈(0,ξ)，结合上述结论整理得到：
[0089][0090]
于是，通过进一步分析可知，最大不变集仅包含闭环系统的平衡轨迹点。因此，根据lasalle不变集原理，使用本实施方式所提的控制器的闭环系统对于目标轨迹是指数稳定的。
[0091]
下面结合实施例作进一步地说明：
[0092]
实施例1、为了验证本实施方式所提的基于koopman算子的软镜扭转迟滞系统驱动模型的有效性，在实验中将输入角度τ的整个输入区间分为三个部分，分别为[30,75]deg、[30,50]deg、[55,75]deg，并在不同的工作区间内测试所提软镜扭转迟滞系统驱动模型的拟合精度。首先进行模型辨识，验证基于koopman算子的软镜扭转迟滞系统驱动模型(公式(7))对于真实系统的逼近精度，辨识所用的数据通过在三段区间上输入扫频三角波信号进行实验采集得到，如图3的实线所示，该组辨识所用数据兼顾了不同工作区间和不同工作频率。经过辨识得到的软镜扭转迟滞系统驱动模型的输出值与系统真实扭转角度的对比结果如图3所示：koopman算子模型的输出值β
model
(实线表示)与扭转角度的真实测量值β
real
(虚线表示)在不同输入区间内变频输入信号下的多重对比结果(近端角扫描速率从0.025rad/s逐渐增加到0.2rad/s；顶层：τ∈[30,75]deg；中间层：τ∈[30,50]deg；底层：τ∈[55,75]deg)，可以看出辨识得到的软镜扭转迟滞系统驱动模型并不能够准确地描述系统的迟滞回环，且传统koopman算子模型的输出值与真实扭转角之间的偏差随着输入频率变高而增加。接下来测试软镜扭转迟滞系统驱动模型的性能，所提迟滞系统驱动模型和后续控制器(公式(20))中所用的集元滤波法的参数经实验测试调节为：
[0093]
[0094]
为了定量地评价本实施例所提基于数据的软镜扭转迟滞系统驱动模型的集元滤波器与观测函数(公式(6))结合后驱动模型的拟合精度，这里定义如下两个模型拟合指标进行对比：
[0095][0096][0097]
软镜扭转迟滞系统驱动模型验证的实验结果如图4所示{本实施例驱动模型的估计值β
est
(短型虚线)，koopman算子模型的输出值β
model
(点虚线)和扭转角度的真实测量值β
real
(实线)在不同输入区间内变频输入信号下的多重对比结果(近端角扫描速率从0.025rad/s逐渐增加到0.2rad/s)；顶层：τ∈[30,75]deg；中间层：τ∈[30,50]deg；底层：τ∈[55,75]deg)}。从图4可以看出，本实施例所提出的基于koopman算子的主动建模技术成功缩小了候选观测向量(公式(6))的有限维逼近误差。相应定量指标(公式(26))和(公式(27))的对比结果列于表1中，
[0098]
表1：对应图4中数据的指标统计与
[0099][0100]
不难发现软镜扭转迟滞系统驱动模型的平均模型近似偏差减少了8倍以上，主动建模方法极大提高了有限维koopman算子模型的精确度。
[0101]
实施例2
[0102]
为了测试本实施例所提控制方法的性能，在输尿管软镜操作机器人平台上仅控制软镜的扭转运动进行了三组对比实验。为了保证控制器的性能，经过实验测试和调节，本实施例所提控制器(公式(20))的控制增益选取为k
p
＝1.8，预设安全边界ξ＝41，所需的估计器参数与(公式(25))中的参数相同(这里需要注意的是，实验中选取41deg为系统的安全约束，以观察所提控制器的安全强化表现)。为了进行对比实验，选择滑模控制(sliding mode control，简称smc)方法和精确模型控制(exact model control，简称emc)与本实施例所提的控制策略进行比较，所选两种对比方法基于所提数据驱动模型的具体表达式如下：
[0103]
1.滑模控制器
[0104][0105]
其中k1表示正的控制增益，为一个满足的正常数。
[0106]
2.精确模型控制器
[0107][0108]
经过多次实验和调节，控制器(公式(20))中的控制参数选取为：k1＝k
p
＝1.8，这里值得一提的是，系统的控制量为近端扭转角度，而伺服电机的位置控制响应速度不是非常迅速，因此选取的滑模控制器并不需要对符号函数进行近似。为了展示本技术
所提方法的定位和跟踪性能，在以下几种情况下进行实验。
[0109]
在实验一中为软镜末端设置了固定的镇定目标，测试了所提控制器的定位控制精度。为了进一步验证所提控制方法的跟踪控制性能，在实验二和实验三中，分别设置了具有连续速度的时变跟踪轨迹(正弦波轨迹)和存在速度突变的时变跟踪轨迹(三角波轨迹)，用来测试所提控制策略在不同轨迹跟踪情况下的表现。总而言之，三种情况的对比实验结果可以共同验证所提控制器的有效性和鲁棒性。
[0110]
实验一：镇定实验：软镜末端的扭转角度的期望值设置为30deg。
[0111]
实验二：正弦波跟踪实验：目标正弦跟踪轨迹的频率、初始相位、幅度和偏移分别设置为0.025hz,-π/2,10deg,30deg，其表达式如下：
[0112][0113]
实验三：三角波跟踪实验：目标三角波跟踪轨迹的频率、初始相位、幅度和偏移分别设置为0.025hz,-π/2,10deg,30deg。
[0114]
为了量化说明本技术所提的方法在的控制性能，本专利定义了如下四种性能指标：
[0115]
整个控制过程中绝对跟踪误差|e|的平均值，即
[0116]evar
：整个控制过程中跟踪误差的方差，即
[0117]
|e|
max
：2s之后的最大跟踪误差，即|e|
max
＝max
t＞2
{|e|}
[0118]erg
：2s之后的跟踪误差范围，即|e|
rg
＝max
t＞2
{e}-min
t＞2
{e}
[0119]
实验一定位控制的实验结果如图5所示(参考轨迹：黑色横向虚线；本技术所提方法：实线；对比emc方法：点虚线；对比smc方法：短型虚线)，从图5中可以看出，通过使用所提的控制方法，扭转角β可以在2s内迅速抵达预设的目标角度30deg。此外，本技术所提的控制策略在软镜扭转运动的镇定控制中的抖动很小，且几乎没有超调和残余误差。对之相比，两种对比方法的调节速度要慢得多，并且在存在明显的震荡和超调。这主要是由于参考数据驱动模型的时变近似误差的存在，精确模型控制和具有定常边界的滑模控制的输入角度会持续波动。实验一对应的量化指标的统计结果如表2所示，
[0120]
表2：实验一中的性能指标
[0121][0122]
本技术所提控制方法的绝对定位误差的平均值为0.4deg，而对比方法的对应指标大于1.2deg，且残余误差的范围e
rg
更是所提方法的3倍。所提的安全强化控制方法通过引入反馈抑制项，使得稳态后的最大定位残差|e|
max
仅为0.8deg，远小于对比方法中的至少3.5deg。此外，所提控制方法的平滑度指标e
var
也远小于对比方法。因此，本技术所提的控制
策略可保证镇定控制的稳定性，且定位控制的残余误差很小，其静态性能相比传统滑模控制器更加优越。
[0123]
实验二和实验三的实验结果如图6(参考轨迹：黑色虚线；本技术所提方法：实线；对比emc方法：点虚线；对比smc方法：短型虚线)和图7所示(参考轨迹：黑色虚线；本技术所提方法：实线；对比emc方法：点虚线；对比smc方法：短型虚线)。针对正弦波或三角波的目标轨迹，所提控制方法的跟踪过程很平滑，几乎没有抖振和超调，且软镜末端的扭转角度β可以很好地跟踪两种目标轨迹，具有较高精度的跟踪效果；相反地，两种对比方法在跟踪过程中都有明显的跟踪误差和系统抖振。特别地，对比实验结果中存在明显的超调现象，且状态量的最大值超出了给定的安全边界41deg。从图6和7中还可以看出，由于反馈抑制项的存在，所提安全强化控制方法的跟踪结果始终满足扭转角度的安全边界约束β(t)《41deg，且其输入角度τ(t)在末端扭转角度接近安全边界时，相比两种对比方法有明显的降低。
[0124]
跟踪实验对应的量化指标的统计结果如表3和4所示。
[0125]
表3：实验二中的性能指标
[0126][0127]
表4：实验三中的性能指标
[0128][0129]
从表3和表4中可以看出，本技术所提控制方法的绝对跟踪误差的平均值均为0.5deg，而对比控制方法的对应指标均大于1.1deg，具有较大的稳态跟踪误差，且跟踪误差范围e
rg
至少是所提控制方法的2倍。此外，所提方法的稳态跟踪误差的最大值|e|
max
不超过1.9deg，而对比方法的最大跟踪误差几乎是所提方法的2倍。特别地，跟踪结果的平滑度指标e
var
还反映出所提方法在稳态跟踪期间具有较小的振荡。综上所述，可以看出本技术所提控制策略具有令人满意的跟踪控制性能和快速的瞬态响应，并且在强化软镜扭转迟滞系统驱动模型的边界约束安全性的同时，也可以提高系统的鲁棒性。
[0130]
本发明已以较佳实施案例揭示如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可以利用上述揭示的结构及技术内容做出些许的更动或修饰为等同变化的等效实施案例，均仍属本发明技术方案范围。