1.一种结合刚柔混合特性的软镜扭转运动自适应控制方法，其特征在于：包含
步骤一、根据软镜操作机器人的刚柔混合特性，建立扭转运动的刚柔混合动力学模型

其中，M表示软镜质量项，β表示软镜末端扭转角度，表示软镜末端扭转角速度，表示软镜末端扭转角加速度，D(·)表示软镜离心力与哥氏力项，Dm(·)表示与输入、输出扭转角度变化相关的扰动，H(·)表示软镜重力项，τ表示刚性关节中直流无刷电机的输入扭矩；
步骤二、基于扭转运动的刚柔混合动力学模型，构建目标误差动力学方程

其中，ε为由于不确定性导致的估计误差，Kc为控制增益，Ksw为符号增益，e为扭转角的跟踪误差；
步骤三、设计基于时滞估计的控制律

μ＝μ1+μ2

其中，为控制律中惯量增益，为自适应增益，为调节增益，r1，r2均为常数且满足-1<r1，r2-1<1，λ1，λ2，p也均为常数且满足λ1，λ2，p均大于0，sgn(·)表示符号函数，构造s作为系统的辅助误差信号，α1,α2,ω1,ω2为常数，μ1为补偿的动力学项，μ2为调节用的幅值函数，即设计自适应时滞控制器，保证软镜在扭转运动中软镜末端被精确跟踪。
2.根据权利要求1所述一种结合刚柔混合特性的软镜扭转运动自适应控制方法，其特征在于：步骤一的具体过程为：将刚性关节中电机驱动机构欧拉-拉格朗日动力学模型与软镜名义动力学模型结合，建立扭转运动的刚柔混合动力学模型；
欧拉-拉格朗日动力学模型如下：

公式(1)中N表示刚性关节质量项，θ表示刚性关节扭转角度，C(·)表示刚性关节离心力与哥氏力项，G(·)表示刚性关节重欧式力项，τ表示刚性关节中直流无刷电机的输入扭矩，τd表示两端扭转位置偏差产生的动态镜体扭转应力矩；
软镜名义动力学模型如下：

公式(2)中M表示软镜质量项，β表示软镜末端扭转角度，D(·)表示软镜离心力与哥氏力项，Dm(·)表示与输入、输出扭转角度变化相关的扰动，H(·)表示软镜重力项；
由公式(1)和公式(2)可得到，扭转运动的刚柔混合动力学模型如下：
。
3.根据权利要求2所述一种结合刚柔混合特性的软镜扭转运动自适应控制方法，其特征在于：步骤二的具体过程为：
首先引入对角增益矩阵将公式(3)整理为公式(4)；

将所有的未知动态项用余项h(t-Ts)表示；

上式中为余项的估计值，和分别表示在时滞为Ts时τ，的时滞值，对于给定末端扭转角度的期望值βd，定义扭转角的跟踪误差为e＝βd-β；
由上可知，时滞控制方案设计为：

其中，sat(e)是一个饱和函数，结合公式(7)的时滞控制输入和公式(4)集总后的动力学方程，可整理得到闭环系统的目标误差动力学方程；

式中：ε为估计误差，Kc为控制增益，Ksw为符号增益。
4.根据权利要求3所述一种结合刚柔混合特性的软镜扭转运动自适应控制方法，其特征在于：步骤三的具体过程为：
定义流形

其中，r1，r2均为常数且满足-1<r1，r2-1<1，λ1，λ2，p也均为常数且满足λ1，λ2，p均大于0，sgn(·)表示符号函数，构造s作为系统的辅助误差信号，是s导数，α1,α2,ω1,ω2为常数；
D(·)是分数阶微分算子，
其定义为：
其中，算子定义为：
且有n-1<α≤n,n∈N，[·]表示Gaussian取整符号；
Γ(·)表示Gamma方程，其定义：

设计得到的基于时滞估计的控制律如下：

其中，为控制律中惯量增益，为自适应增益，为调节增益，μ1为补偿的动力学项，μ2为调节用的幅值函数，由此，实现软镜扭转运动自适应跟踪控制。
5.根据权利要求4所述一种结合刚柔混合特性的软镜扭转运动自适应控制方法，其特征在于：所述公式(15)中的具体表达式表示为：

上式为自适应更新律的化简形式，其中，符号的具体形式为：

上式中κ为自适应更新律的增益参数，kη自适应更新律的调节参数，λc为自适应更新律的符号参数，ξmax表示自适应参数ξ在自调节过程中允许的最大值，δ表示一个用来克服奇异点问题的正常数，Δ为在后续控制器稳定性分析中设计的边界；
进一步地，惯量增益可以写作如下形式：

上式中分别为控制器增益M,k的初始值，κ1,κ2均为待调节的自适应更新律参数；
为了满足和时滞控制稳定性的约束条件(公式9)，κ的数值可以根据kη来选取，当s的数值到达障碍边界±(λc+Δ)时，增益自调节速度可以到达最大值参数选取的具体表达式如下：

μ2可以用来调节自适应公式(16)的幅值大小，其具体形式为：
。
6.根据权利要求5所述一种结合刚柔混合特性的软镜扭转运动自适应控制方法，其特征在于：还包括稳定性分析：
选取Lyapunov候选函数如下：

上式中
对公式(23)中的V求导，并将公式(4)和公式(16)代入其中，整理得到：

公式(24)中，
将公式(15)代入公式(24)中，合并同类项后可以整理得到：

公式(26)中，
分两种情况讨论的性质：
情况一：当时，即满足a)且||s||＜Δ,b)则公式(26)可以重写为如下形式：

上式中max{-||s||υ1(ξ),υ2(s)}＞0；
情况二：当即满足a)、且||s||≥Δ，b)、则公式(26)可重写为如下形式：

公式(29)是公式(28)的约束条件，下面讨论公式(29)的性质即可；进一步将公式(29)重新表示为：

当时，显然有则标量V可以连续递减到0，闭环系统此时渐近稳定，当时，如果有根据闭环系统的稳定性分析结论，可推导出|s|可以收敛到如下区间：

反之，当或时，本身满足|s|≤Δ的条件，式中的Δ即为所提自适应公式(16)中的边界值，于是，当辅助信号|s|有界时，系统的跟踪误差e也是有界的。

技术领域
[0001]本发明属于医疗机器人控制技术领域，具体地涉及一种结合刚柔混合特性的软镜扭转运动自适应控制方法。
背景技术
[0002]基于软镜操作技术实现的经人体自然腔道内镜手术作为一种典型的微创甚至无创手术在最近几年受到了广泛的关注并应用于多种疾病诊查与外科手术中。软镜末端的弯曲运动、软镜镜体的扭转运动和推进运动共同实现了软镜机器人末端的三自由度运动。当前软镜操作机器人在控制中存在弯曲扭转耦合的难点，软镜末端的镜体扭转运动的控制还比较少，尤其是针对软镜在引导鞘内扭转的运动控制方法还几乎没有被研究过。
[0003]由于软镜制造材料的柔性特性以及软镜操作机器人系统的刚柔混合特性，其末端的运动中常常伴随迟滞、速率相关动态和时变参数等复杂非线性特性，对实现软镜末端运动的精确控制造成了很大困难。在实际使用中，软镜操作机器人系统一般使用医学影像实现软镜末端在人体内的定位和导航，但是图像处理和大量数据信号的传输会给系统带来明显的时滞现象，医学影像处理速度较慢时系统是一个典型的时滞系统。
[0004]对于软镜机器人的扭转运动而言，特有的结构带来了复杂迟滞、时变特性和未知动态，软镜再引导鞘内扭转运动受干扰影响大，内部拉线振动和外力干扰增加了控制难度，整体机器人系统“刚性连接关节+软镜弹性扭转”的动力学模型复杂且难以有效辨识，在临床中使用医学影像导航存在的信号时滞，都对软镜在引导鞘内的精确控制提出了很大的挑战。
发明内容
[0005]本发明为克服现有技术的不足，提供一种结合刚柔混合特性的软镜扭转运动自适应跟踪控制方法。该方法克服了未知摩擦扰动、未建模动态和时变参数造成的时滞影响，以及传统软镜操作机器人的刚柔混合特性，提高了软镜末端扭转运动定位和跟踪精度。
[0006]一种结合刚柔混合特性的软镜扭转运动自适应控制方法包含
[0007]步骤一、根据软镜操作机器人的刚柔混合特性，建立扭转运动的刚柔混合动力学模型
[0008]
[0009]其中，M表示软镜质量项，β表示软镜末端扭转角度，表示软镜末端扭转角速度，表示软镜末端扭转角加速度，D(·)表示软镜离心力与哥氏力项，Dm(·)表示与输入、输出扭转角度变化相关的扰动，H(·)表示软镜重力项，τ表示刚性关节中直流无刷电机的输入扭矩；
[0010]步骤二、基于扭转运动的刚柔混合动力学模型，构建目标误差动力学方程
[0011]
[0012]其中，ε为由于不确定性导致的估计误差，Kc为控制增益，Ksw为符号增益；
[0013]步骤三、设计基于时滞估计的控制律
[0014]
[0015]μ＝μ1+μ2
[0016]
[0017]
[0018]
[0019]其中，为控制律中惯量增益，为自适应增益，为调节增益，μ1为补偿的动力学项，μ2为调节用的幅值函数，即设计自适应时滞控制器，保证软镜在扭转运动中软镜末端被精确跟踪。
[0020]本发明相比现有技术的有益效果是：
[0021]本发明所提的自适应控制方法不仅具有良好的定位和跟踪控制性能，对引导鞘内不同摩擦力的干扰也具有良好的鲁棒性。本发明克服了未知摩擦扰动、未建模动态和时变参数造成的时滞影响，以及传统软镜操作机器人的刚柔混合特性，提高了软镜末端扭转运动定位和跟踪精度。
[0022]下面结合附图和实施例对本发明的技术方案作进一步地说明：
附图说明
[0023]图1为本发明结合的软镜操作机器人姿态状态图；
[0024]图2为本发明结合的软镜操作机器人的刚性关节和软镜末端的相互作用关系图；
[0025]图3为实施例1中第一种实验的刚性关节扭矩、软镜末端扭转角和跟踪误差的比对结果图；
[0026]图4为实施例1中第一种实验下的自适应增益的调节过程和辅助误差信号的变化情况结果图；
[0027]图5为实施例1中第二种实验下刚性关节扭矩、软镜末端扭转角和跟踪误差的比对结果图；
[0028]图6为实施例1中第二种实验下的自适应增益的调节过程和辅助误差信号的变化情况结果图；
[0029]图7为实施例1中第三种实验下的刚性关节扭矩、软镜末端扭转角和跟踪误差的比对结果图
[0030]图8为实施例1中第三种实验下的自适应增益的调节过程和辅助误差信号的变化情况结果图；
[0031]图9为实施例2中变摩擦力实验下刚性关节扭矩、软镜末端扭转角和跟踪误差的比对结果图；
[0032]图10为实施例2中变摩擦力实验下的自适应增益的调节过程和辅助误差信号的变化情况结果图。
具体实施方式
[0033]下面将结合附图对本发明技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本发明的技术方案，因此只作为示例，而不能以此来限制本发明的保护范围。
[0034]需要注意的是，除非另有说明，本申请使用的技术术语或者科学术语应当为本发明所属领域技术人员所理解的通常意义。
[0035]为了更清晰地说明本发明的实现过程，现将本发明的具体实施方式介绍如下。
[0036]结合图1-图2说明：一种结合刚柔混合特性的软镜扭转运动自适应控制方法包含：
[0037]步骤一、根据软镜操作机器人的刚柔混合特性，建立扭转运动的刚柔混合动力学模型
[0038]
[0039]其中，M表示软镜质量项，β表示软镜末端扭转角度，表示软镜末端扭转角速度，表示软镜末端扭转角加速度，D(·)表示软镜离心力与哥氏力项，Dm(·)表示与输入、输出扭转角度变化相关的扰动，H(·)表示软镜重力项，τ表示刚性关节中直流无刷电机的输入扭矩；
[0040]步骤二、基于扭转运动的刚柔混合动力学模型，构建目标误差动力学方程
[0041]
[0042]其中，ε为由于不确定性导致的估计误差，Kc为控制增益，Ksw为符号增益；
[0043]步骤三、设计基于时滞估计的控制律
[0044]
[0045]μ＝μ1+μ2
[0046]
[0047]
[0048]
[0049]其中，为控制律中惯量增益，为自适应增益，为调节增益，μ1为补偿的动力学项，μ2为调节用的幅值函数，即设计自适应时滞控制器，保证软镜在扭转运动中软镜末端被精确跟踪。
[0050]本实施方式通过刚柔混合动力模型、时滞控制律和自适应时滞控制器的搭建，有效地克服了未知摩擦扰动、未建模动态和时变参数造成的时滞影响，以及软镜操作机器人刚柔混合特性的特点，达到跟踪误差趋于零，实现了软镜末端扭转运动定位和跟踪精度的提高。
[0051]进一步来说，步骤一的具体过程为：将刚性关节中电机驱动机构欧拉-拉格朗日动力学模型与软镜名义动力学模型结合，建立扭转运动的刚柔混合动力学模型；
[0052]欧拉-拉格朗日动力学模型如下：
[0053]
[0054]公式(1)中N表示刚性关节质量项，θ表示刚性关节扭转角度，C(·)表示刚性关节离心力与哥氏力项，G(·)表示刚性关节重欧式力项，τ表示刚性关节中直流无刷电机的输入扭矩，τd表示两端扭转位置偏差产生的动态镜体扭转应力矩；
[0055]软镜名义动力学模型如下：
[0056]
[0057]公式(2)中M表示软镜质量项，β表示软镜末端扭转角度，D(·)表示软镜离心力与哥氏力项，Dm(·)表示与输入、输出扭转角度变化相关的扰动，H(·)表示软镜重力项；
[0058]由公式(1)和公式(2)可得到，扭转运动的刚柔混合动力学模型如下：
[0059]
[0060]进一步地，针对系统的时滞效应问题，设计时滞控制方案，得到时滞控制框架，步骤二的具体过程为：
[0061]首先引入对角增益矩阵将公式(3)整理为公式(4)；
[0062]
[0063]将所有的未知动态项用余项h(t-Ts)表示；
[0064]
[0065]
[0066]上式中为余项的估计值，和分别表示在时滞为Ts时τ，的时滞值，对于给定末端扭转角度的期望值βd，定义扭转角的跟踪误差为e＝βd-β；
[0067]由上可知，时滞控制方案设计为：
[0068]
[0069]其中，sat(e)是一个饱和函数，结合公式(7)的时滞控制输入和公式(4)集总后的动力学方程，可整理得到闭环系统的目标误差动力学方程；
[0070]
[0071]式中：ε为估计误差，Kc为控制增益，Ksw为符号增益。
[0072]通过合理的选取增益Kc，Ksw，时滞控制方案的稳定性由以下不等式保证：
[0073]
[0074]更进一步地，自适应时滞控制器设计中，步骤三的具体过程为：
[0075]定义流形
[0076]
[0077]
[0078]其中，r1，r2均为常数且满足-1<r1，r2-1<1，λ1，λ2，p也均为常数且满足λ1，λ2，p均大于0，sgn(·)表示符号函数，构造s作为系统的辅助误差信号，是s导数，α1,α2,ω1,ω2为常数；
[0079]D(·)是分数阶微分算子，
[0080]其定义为：
[0081]其中，算子定义为：
[0082]且有n-1<α≤n，n∈N，[·]表示Gaussian取整符号；
[0083]Γ(·)表示Gamma方程，其定义：
[0084]
[0085]设计得到的基于时滞估计的控制律如下：
[0086]
[0087]其中，为控制律中惯量增益，为自适应增益，为调节增益，μ1为补偿的动力学项，μ2为调节用的幅值函数，由此，实现软镜扭转运动自适应跟踪控制。
[0088]上述公式(15)中，具体表达公式表示为：
[0089]
[0090]上式为自适应更新律的化简形式，其中，符号的具体形式为：
[0091]
[0092]
[0093]上式中κ为自适应更新律的增益参数，kη自适应更新律的调节参数，λc为自适应更新律的符号参数，ξmax表示自适应参数ξ在自调节过程中允许的最大值，δ表示一个用来克服奇异点问题的正常数，Δ为在后续控制器稳定性分析中设计的边界，
[0094]进一步地，惯量增益可以写作如下形式：
[0095]
[0096]上式中分别为控制器增益M,k的初始值，κ1,κ2均为待调节的自适应更新律参数。
[0097]为了满足和时滞控制稳定性的约束条件，κ的数值可以根据kη来选取，当s的数值到达障碍边界±(λc+Δ)时，增益自调节速度可以到达最大值参数选取的具体表达式如下：
[0098]
[0099]
[0100]μ2可以用来调节自适应公式(16)的幅值大小，其具体形式为：
[0101]
[0102]至此满足基于时滞估计的控制律的参数及运行方式设计完成。
[0103]基于上述实施方案中，对上述基于时滞估计的控制律进行稳定性分析：
[0104]选取Lyapunov候选函数如下：
[0105]
[0106]上式中
[0107]对公式(23)中的V求导，并将公式(4)和公式(16)代入其中，整理得到：
[0108]
[0109]公式(24)中，
[0110]将公式(15)代入公式(24)中，合并同类项后可以整理得到：
[0111]
[0112]公式(26)中，
[0113]分两种情况讨论的性质：
[0114]情况一：当时，即满足a)且||s||＜Δ,b)则公式(26)可以重写为如下形式：
[0115]
[0116]上式中max{-||s||υ1(ξ),υ2(s)}＞0；
[0117]情况二：当即满足a)、且||s||≥Δ，b)、则公式(26)可重写为如下形式：
[0118]
[0119]公式(29)是公式(28)的约束条件，下面讨论公式(29)的性质即可；进一步将公式(29)重新表示为：
[0120]
[0121]
[0122]当时，显然有则标量V可以连续递减到0，闭环系统此时渐近稳定，当时，如果有根据闭环系统的稳定性分析结论，可推导出|s|可以收敛到如下区间：
[0123]
[0124]反之，当或时，本身满足|s|≤Δ的条件，式中的Δ即为所提自适应公式(16)中的边界值，于是，当辅助信号|s|有界时，系统的跟踪误差e也是有界的。
[0125]稳定性分析表面，上述实施方式所提的控制策略可以很好地处理软镜操作机器人扭转运动中的刚柔混合特性，并实现令人满意的跟踪性能和更加迅速的瞬态响应。
[0126]下面通过实施例对本发明构思作进一步地说明：
[0127]实施例1：不同轨迹下对比实验
[0128]为对比本实施例的控制方法与PID、RISE算法在不同任务条件下的跟踪性能，两个控制器分别为公式(33)和公式(34)：
[0129]
[0130]
[0131]其中，RISE辅助误差系统设计为：
[0132]
[0133]
[0134]其中，ef为辅助变量
[0135]以下三种典型轨迹为例进行实验比对：
[0136]1、跃迁轨迹(对应第一种实验)
[0137]βd(t)＝30deg (37)
[0138]2、正弦波轨迹(对应第二种实验)
[0139]
[0140]3、三角波轨迹(对应第三种实验)，目标三角波参考轨迹的频率、初始相位、幅值和轨迹偏移分别为0.025Hz、π＝2、10deg和30deg。
[0141]实验结果如图3-8所示。由图3中可以看出(图3中最上图的黑色虚线参考轨迹线，中间图的黑色虚线为目标跟踪误差，上中下三个图中，实线表示本申请所提的控制器下性能曲线、短型虚线表示RISE控制器下的性能曲线、点划虚线表示PID控制器下的性能曲线)，通过应用所提的控制策略，软镜末端的扭转角度β可以在2s内迅速抵达预设的30deg的期望值；与之相比较，对比所用的两种无模型控制方法需要至少10s才能将软镜末端扭转到预设角度，验证了所提控制方法具有更加快速的瞬态响应。此外，在扭转角度β稳定在目标角度后，本实施例提出的方法几乎没有残余误差，然而对比方法存在明显的残余振荡且会持续超过20s。特别地，从图4中可以看出(此图是图3情况下，本申请所提自适应策略(公式(16))中自适应增益的调节过程和辅助误差信号s的变化曲线图)，自适应增益在最大边界下方轻微地波动，且自调节速度在稳定位置时趋于0，辅助误差信号s在软镜末端扭转到达期望值之后也迅速收敛到0。因此，本实施例所提的增益自适应更新律可保证系统稳态响应的性能。
[0142]本实施例所提的控制方法在跟踪正弦波轨迹和三角波轨迹后相比对比方法的跟踪误差很小，结果如图5(图中最上面的图黑色虚线表示参考轨迹线，中间图的黑色虚线表示目标跟踪误差曲线，上中下三个图中，实线表示本申请所提控制器下性能曲线，短型虚线表示RISE控制器下的性能曲线，点划虚线表示PID控制器下的性能曲线)及图7所示(图中最上面的图黑色虚线表示参考轨迹线，中间图的黑色虚线表示目标跟踪误差曲线，上中下三个图中，实线表示本申请所提控制器下性能曲线，短型虚线表示RISE控制器下的性能曲线，点划虚线表示PID控制器下的性能曲线)。不仅如此，从图6(此图是图5情况下，本申请所提自适应策略(公式(16))中自适应增益的调节过程和辅助误差信号s的变化曲线图)和图8(此图是图7情况下，本申请所提自适应策略(公式(16))中自适应增益的调节过程和辅助误差信号s的变化曲线图)中可以发现，当跟踪过程即将到达参考轨迹的极值点时，增益自适应律的调节速度变化大，因此刚柔混合系统的迟滞特性在所提的无模型控制策略下得到了一定程度的抑制。上述对比实验说明，本实施例方法所提的控制策略可以很好地处理软镜操作机器人扭转运动中的刚柔混合特性，并实现令人满意的跟踪性能和更加迅速的瞬态响应。
[0143]实施例2：变摩擦力实验
[0144]在本组实验中，为了进一步验证所提控制方法在实际应用中面对各种外部干扰的有效性和鲁棒性，将通过调节旋钮来改变软镜引导鞘和镜体之间的预紧压力，可以分别在20s、40s和60s的时候改变软镜扭转所受到的摩擦力。这里仍使用PID控制器和RISE控制器作为对比方法，在改变摩擦力后也将同时改变扭转角β的镇定目标，预设的期望轨迹的幅度分别设置为25deg和40deg，而参考轨迹的频率与摩擦力的变化频率一致为0.05Hz。
[0145]实验二的实验结果如图9(图中最上面的图黑色虚线表示参考轨迹线，中间图的黑色虚线表示目标跟踪误差曲线，上中下三个图中，实线表示本申请所提控制器下性能曲线，短型虚线表示RISE控制器下的性能曲线，点划虚线表示PID控制器下的性能曲线)和图10(此图是图9情况下，本申请所提自适应策略(公式(16))中自适应增益的调节过程和辅助误差信号s的变化曲线图)所示。从图9中可以看出，在镜体所受摩擦变化后，所提的控制器仍可以保持快速准确的镇定性能；相比较而言，对比方法需要更长的时间将末端扭转角度调节到目标位置，并且存在较大的跟踪误差。特别注意的是，自适应参数ξ可以快速适应变化后的环境，具有明显剧烈变化的调节速度，用以克服未知摩擦力变化产生的扰动。因此，本实施例所提的控制方法在应对外部扰动/环境变化时具有令人满意的自调节性能，且仍能保证扭转TSAM机构的精确控制和稳定性。
[0146]本发明已以较佳实施案例揭示如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可以利用上述揭示的结构及技术内容做出些许的更动或修饰为等同变化的等效实施案例，均仍属本发明技术方案范围。