一种基于二阶滑模算法的柔性针智能路径跟踪方法与流程

本发明属于智能医疗领域，涉及一种基于二阶滑模算法的柔性针智能路径跟踪方法。

背景技术：

近些年来，随着医疗器械的不断发展，智能医疗领域得到了各方面的高度关注。而柔性针作为新型医疗器械，由于具有较高的灵活性和广泛的适用性，成为众多医疗机器人的重要组成部分，同时也吸引了众多研究机构，在此领域进行深入研究。

柔性针可在人体软组织中能够有规律的改变运动轨迹，能在人体当中绕过重要神经以及内脏组织对目标区域进行药物注射和活检，使用非常方便。且柔性针的本质是一个具有较小刚度的柔性材料，当其在软组织中正常行进时，针尖自身的角度会给针尖带来一个法向反作用力，使得针尖产生一个弯曲的行进趋势，最终绕过重要器官。

在智能柔性针医疗器械中，常使用超声波作为传感器，在内部环境得到清晰感知的情况下，通过计算机和智能算法规划出合理的轨迹。在轨迹确定之后，通过控制器对执行电机的控制来改变柔性针的状态，从而对路径进行实时跟踪，最终达到目标位置。但是人体软组织密度是在时刻变化的，在不同时态下的柔性针的行进状态也有所不同。而传统的线性定常控制器并不能根据柔性针行进工况的变化进行智能调节，所以非线性时变控制模型的建立是其中的关键问题。

近年来，滑模控制策略得到快速的发展，由于其有较好的鲁棒性和变结构特性，从而在控制领域得以广泛应用。但是滑模控制器在状态转变时会产生较强的抖振，并在变结构时无法连续控制，这会降低医疗器械的稳定性和精确性，并对活检以及治疗的效果产生影响。

技术实现要素：

为了解决上述问题，建立起一个精度高，稳定性好，使用条件广泛的柔性针医疗器械，并使柔性针能够较好的完成对规划路径的跟踪，本发明提出了一种基于二阶滑模算法的柔性针智能路径跟踪方法。

具体步骤如下：

步骤一、根据柔性针医疗器械进给过程中的物理特性，建立柔性针针尖和目标之间的运动学模型。

物理特性是指：在行进过程中，当柔性针的行进速度为常数值时，通过控制柔性针针尖的纵向加速度来控制柔性针整体轨迹。柔性针的加速度矢量大小始终保持不变，而方向会随着柔性针针尖斜截面的变化而不断变化，则在时域内针尖斜截面的矢量积分决定了柔性针为一段时间内的总体方向。

针尖和目标之间的运动学模型方程为：

l为柔性针针尖和目标之间的相对距离；v表示柔性针针尖的行进速度；η为柔性针针尖和目标之间的前置夹角；θ表示柔性针针尖的速度航向角，a表示针尖在软组织行进过程中的纵向加速度。

步骤二、在已知工作路径的工况下，通过运动学模型建立针目坐标系下柔性针的二维极坐标；

针目坐标系的二维平面内的任意一点，柔性针的二维极坐标表示为(l，λ)；

λ为目标视线角，取值为柔性针针尖的速度航向角θ与针尖和目标之间的前置夹角η之和。

步骤三、将柔性针在理想路径坐标系下的坐标值与已知工作路径坐标系下的坐标值进行作差，构建基于坐标误差的跟踪评价指标e。

柔性针在理想路径坐标系上任一点的理想坐标，表示为(ld，λd)。

则两坐标值之间的误差分别表示为：

为了平衡两误差之间的权重，引入系数c作为跟踪评价指标权重系数，重构跟踪评价指标e为：

e＝e1+ce2

c为正实数。

步骤四、将跟踪评价指标e输入到二阶滑模控制系统中，通过控制器对跟踪评价指标e进行智能控制及优化，使柔性针轨迹跟踪的误差最小。

具体步骤为：

步骤401、利用跟踪评价指标e构建滑模变量s；

如下：

c1为正实数；

步骤402、结合滑模变量s和运动学模型方程，重构滑模变量并求其时间积分；

ω是控制系统的扰动；β为常量；u是控制系统的输入量，取值为柔性针在软组织中行进的纵向加速度a；ξ和α均是可调参数；k和p为重构滑模变量而建立起的中间变量。

步骤403、通过对输入量u的可调参数ξ和α进行调整，使滑模变量满足李雅谱诺夫方程组，则滑模变量和其变化率在有限时间内趋于稳定，即完成柔性针的智能路径追踪。

李雅谱诺夫方程为：

式中x为输入的比例项和积分项的拆分，公式如下：

对李雅谱诺夫方程求时间微分得：

当可调参数ξ和α的取值如下时：

滑模变量和其变化率在有限时间内趋于稳定，柔性针的控制系统为稳定状态，在行进过程中的误差会在滑模控制的作用下不断趋于零，即可完成柔性针的路径跟踪。

本发明的优点在于：

本发明利用二阶滑模控制算法这一新型控制策略来完成柔性针医疗器械的轨迹跟踪，该方法相较于其他原有方法控制效果更加精确，适用领域更加广泛。

附图说明

图1为本发明柔性针的运动学模型结构图；

图2为本发明一种基于二阶滑模算法的柔性针智能路径跟踪方法原理图；

图3为本发明一种基于二阶滑模算法的柔性针智能路径跟踪方法的流程图。

具体实施方案

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明对柔性针智能路径规划系统的原理如图2所示，首先，建立柔性针运动学模型；通过对针尖实际运动中坐标的观测与理想情况下针尖完成轨迹的相应位置坐标输入到跟踪评价系统当中，构建柔性针路径跟踪评价指标e；在针尖的行进过程中，为了将评价指标维持在合适范围内，则通过评价指标进行滑模面的构建，并设计二阶滑模控制算法来消除系统抖振，进行增加系统的鲁棒性和稳定性；最后根据控制理论的设计原则，对二阶滑模控制系统进行稳定收敛验证，并根据验证原则调整控制系统参数，以让柔性针能够成功到达预期目标。

如图3所示，具体步骤如下：

步骤一、根据柔性针医疗器械进给过程中的物理特性，建立柔性针针尖和目标之间的运动学模型。

针尖和目标之间的运动学模型方程为：

l为柔性针针尖和目标之间的相对距离；v表示柔性针针尖的行进速度；η为柔性针针尖和目标之间的前置夹角；θ表示柔性针针尖的速度航向角，a表示针尖在软组织行进过程中的纵向加速度。

柔性针在人体软组织中行进示意图如图1所示，从基准线逆时针转向速度向量为正；在行进过程中，当柔性针的行进速度v为一常数值时，通过控制柔性针针尖的纵向加速度a来控制柔性针整体轨迹。柔性针的加速度矢量大小始终保持不变，而方向会随着柔性针针尖斜截面的变化而不断变化，则在时域内针尖斜截面的矢量积分决定了柔性针为一段时间内的总体方向。

步骤二、在已知工作路径的工况下，通过运动学模型建立针目坐标系下柔性针的二维极坐标；

针目坐标系的二维平面内的任意一点，柔性针的二维极坐标表示为(l，λ)；

λ为目标视线角，取值为柔性针针尖的速度航向角θ与针尖和目标之间的前置夹角η之和。

则重构针目运动学模型如下式所示：

当路径规划系统规划出一条合适的曲线路径时，该曲线用相应的非线性微分方程进行拟合，此时曲线上的坐标为柔性针行进理想坐标，坐标表示为(ld，λd)。

步骤三、将柔性针在理想路径坐标系下的坐标值与已知工作路径坐标系下的坐标值进行作差，构建基于坐标误差的跟踪评价指标e。

由于柔性针在真实运行环境下会受到诸多干扰，导致柔性针在实际行进过程中与理想轨迹有一定偏差，通过相应时刻柔性针坐标与理想轨迹坐标的对比，进行柔性针跟踪效果的评价；两坐标值之间的误差分别表示为：

为了平衡两误差之间的权重，引入系数c作为跟踪评价指标权重系数，重构跟踪评价指标e为：

e＝e1+ce2(4)

c的取值范围为正值，且可为任意实数，则c的取值大小决定了e1，e2对整体误差e的影响。

步骤四、将跟踪评价指标e输入到二阶滑模控制系统中，通过控制器对跟踪评价指标e进行智能控制及优化，使柔性针轨迹跟踪的误差最小。

二阶滑模控制策略是一种改进的滑模控制策略，相较于其他的滑模控制算法，二阶滑模控制器解决了相对阶数为一，变结构时易产生抖振的不足。在这里用于解决在人体软组织当中，智能医疗器械所面临的工况时实变化的问题。滑模算法是变结构算法，它通过设定与滑模面相关的输入为途径，将滑模变量控制在有限时间衰减为零作为目标。

具体步骤为：

步骤401、利用跟踪评价指标e构建滑模变量s；

由于滑模控制最终是以滑模变量的稳定为目标，则在柔性针路径跟踪领域可将设立的整体误差与滑模控制思想进行结合。

首先构建滑模变量，如下：

c1的取值范围为正值，且可为任意实数。滑模变量s与跟踪评价指标e，以及其一阶倒数线性相关；当滑模变量在有限时间内衰减为零时，跟踪评价指标e以及跟踪评价指标e的变化率也将逐渐衰减，从而使柔性针可以完成对理想轨迹的跟踪。

步骤402、结合滑模变量s和运动学模型方程，重构滑模变量并求其时间积分；

为了将滑模变量与柔性针的输入控制量结合起来，达到通过滑模变量反算出柔性针控制量的目的，结合上步构建的滑模变量(5)和柔性针的运动学方程(2)，重构滑模变量并求其时间积分为：

同时由于柔性针控制系统的输入a是线性输入，则可将其由一般控制通式表示，针对式(6)则有ω，u，k和p的表达式如下：

ω是控制系统的扰动，由于外界扰动总是有界，则可以用常量β进行限定；u是控制系统的输入量，取值为柔性针在软组织中行进的纵向加速度a；ξ和α均是可调参数；k和p为重构滑模变量而建立起的中间变量。

经过上述变换，柔性针在工作状态中的跟踪误差便和输入进行了联系与结合，只需调整参数使控制系统稳定，即可完成柔性针的智能路径追踪。

步骤403、通过对输入量u的可调参数ξ和α进行调整，使滑模变量满足李雅谱诺夫方程组，则滑模变量和其变化率在有限时间内趋于稳定，即完成柔性针的智能路径追踪。

在本控制系统中，输入决定了系统的稳定性；柔性针跟踪评价指标e以及跟踪评价指标e的变化率可以在有限时间内同时归零，柔性针完成路径跟踪。

根据控制理论构建李雅谱诺夫方程为：

式中x为输入的比例项和积分项的拆分，公式如下：

为输入的比例项和积分项拆分，对李雅谱诺夫方程求时间微分得：

通过构造可调参数ξ和α的取值，使(8)、(9)两式均小于0，则有控制理论可得，滑模变量和其变化率可以在有限时间内趋于稳定，由限定条件反算可知：

当可调参数ξ和α的取值如上时，柔性针的控制系统为稳定状态，在行进过程中的误差会在滑模控制的作用下不断趋于零，即可完成柔性针的路径跟踪。