CN113867133B - 融合pid控制和预测模型模糊切换的轨迹跟踪控制方法 - Google Patents

Abstract

本发明公开了融合PID控制和预测模型模糊切换的轨迹跟踪控制方法，包括以下步骤：1）在惯性坐标系下建立弧线穿刺时预弯柔性探针针尖运动学模型；2）设计路径跟踪混合控制器，包括针对直线路径的PID预瞄控制器和针对弧线路径的模型预测控制器；3）设计监督器和混合切换模糊控制器实现对路径切换时的混合跟踪控制。本发明根据预弯柔性针穿刺模式采用了基于预瞄PID控制和基于的模型预测控制的混合跟踪方法，通过模糊控制器进行切换，提高了路径切换时的稳定性，减少整体计算强度。

Description

融合PID控制和预测模型模糊切换的轨迹跟踪控制方法

技术领域

本发明涉及一种柔性针路径跟踪控制方法，特别涉及融合PID控制和预测模型模糊切换的轨迹跟踪控制方法。

背景技术

微创手术通过靶向穿刺，可直接对体内病灶区域进行诊断和药物投递，具有创伤小、恢复快等优点。随着现代医疗的不断发展和患者要求的提高，微创手术的快速发展已成为必然趋势，避障的靶向穿刺手术通过柔性针来实现。普通柔性针在穿刺时受到组织的作用力会发生偏转，改变穿刺路径。根据这一特点，可以实现避障的功能。

由于人体内受力的复杂性，穿刺过程中会受到多种误差的影响，因此需要跟踪控制来实现精确的靶向穿刺。对于柔性针的控制，包括针的进给运动和针轴的自转运动。进给运动使柔性针在组织中的穿刺，自转运动可以改变针尖的穿刺方向，通过对两个运动的协调控制，就可以实现一定形式的空间穿刺路径。

常见的路径跟踪控制算法包括预瞄控制、滑模控制和模型预测控制等，其中预瞄控制鲁棒性较差；常规的滑模控制抵抗干扰能力强但是存在输出抖振问题；模型预测控制对模型精度和处理器实时计算能力要求高，在实际应用中有较多限制，且鲁棒控制的设计过程复杂。

2019年张旭明等人提出了基于深度强化学习的斜尖柔性针路径跟踪用网络优化方法(发明专利公开号：CN111048212A)，基于柔性针在路径跟踪中反馈的位置和误差信息，通过深度强化学习并配合目标动作网络一起完成动作网络的训练优化，优化得到的动作网络能够进一步实现对斜尖柔性针三维路径的跟踪，然而深度强化学习的方法对样本的利用率较低，需要长时间训练。2019年张辉等人提出了一种基于二阶滑模算法的柔性针智能路径跟踪方法(发明专利公告号：CN109870907B)，基于实际路径和理想路径的差构建跟踪误差指标，通过二阶滑模算法进行控制优化，在滑模控制下误差趋近于零，然而一般在时限内无法实现，积累后对精度有较大影响。2019年张辉等人提出了一种基于MPC算法的柔性针穿刺路径跟踪控制方法(发明专利公告号：CN110262230B)，基于模型预测控制(MPC)算法建立针对进给速度和曲率的时变预测模型，通过预测模型，滚动优化和反馈校正，实现柔性针按照预设的穿刺路径进行精确穿刺，然而该算法对模型精度和处理器实时计算能力要求高，实时应用中有较多限制。

综上所述，现有方法采用的滑模控制方法抵抗干扰能力强但是存在输出抖振问题，采用的模型预测控制方法对模型精度和处理器实时计算能力要求高，在实际应用中有较多限制，且鲁棒控制的设计过程复杂。

发明内容

本发明的目的是克服现有技术缺陷，提供一种融合PID控制和预测模型模糊切换的轨迹跟踪控制方法，根据预弯柔性针穿刺模式采用了基于预瞄PID控制和基于的模型预测控制的混合跟踪方法，通过模糊控制器进行切换，提高了路径切换时的稳定性，减少整体计算强度。

本发明的目的是这样实现的：一种融合PID控制和预测模型模糊切换的轨迹跟踪控制方法，包括以下步骤：

步骤一：在惯性坐标系下建立弧线穿刺时预弯柔性探针针尖运动学模型；

所述步骤一中的惯性坐标系为以路径组合切线方向为x方向，法线方向为y方向建立的惯性坐标系，y方向的值为挠度w；所述弧线穿刺时预弯柔性探针针尖运动学模型为基于弹簧理论的针转向模型，建立以相切点为圆心，切向为x轴法向为y轴的针轴坐标系，运动学模型如式(1)：

式中a_nl为三次多项式系数，对于不同针芯伸长量l，都有相对应的四个系数a_nl，n＝0,1,2,3时，ω(x)为路径组合切向进给量x处对应的针尖挠度，基于由针芯伸长量引起的曲率变化，得到了与曲率有关的系数矩阵A_nl，系数矩阵A_nl由四个系数a_nl，n＝0,1,2,3组成，阶数为4×l阶；

步骤二：设计路径跟踪混合控制器，包括针对直线路径的PID预瞄控制器和针对弧线路径的模型预测控制器；所述PID预瞄控制器采用基于编码器电机进给量误差值的PID预测控制；所述模型预测控制器采用基于进给量误差及穿刺曲率的模型预测控制；

直线穿刺时的PID预瞄控制器采用增量式PID控制方法，控制模型如下：

定义***采样周期为T，k为当前采样时刻；u(k)为k时刻的***输出量，Δu(k)为***的输出增量，e(k)为***误差由电机编码器获得作为控制器的输入量；优化系数K_p为比例系数，T_i为积分时间，T_d为微分时间；

弧线穿刺时采用模型预测控制器，根据预弯柔性探针针尖运动学模型，转化为状态空间方程，建立如式(3)观测模型：

式中x为路径组合切向进给量，为其对时间求导，w为挠度，/>为其对时间求导，θ为针尖与切向的夹角，l为针芯伸长量，v为穿刺速度，A_l为伸长量为l时的系数矩阵，得到状态变量ξ＝[x w]^T，输入控制量u＝[l v]^T；

得到离散化的预测增量模型如式(4)：

x(k+1)＝Ax(k)+Bu(k) (4)

式中x(k)为k时刻的状态量，u(k)为k时刻的***输出量，通过预测增量模型在定义的控制时域下进行系数A和B的滚动优化，得到最优的初始状态量及控制量，定义控制时域为5；

建立基于预测误差和控制误差的目标函数如式(5)，求解预测变化及控制变化的矩阵二范数之和的最小值；

式中E_p为预测时域下得到的位置误差矩阵，阶数为预测时域值；E_u为控制时域下得到的输入误差矩阵，阶数为控制时域乘输入控制量个数；Q和R分别为与E_p和E_u相同阶的带系数的单位阵，系数分别为5和1；

通过拟牛顿法得到目标函数的最小值时的控制量，实现对针尖目标的跟踪控制；

步骤三：设计监督器和混合切换模糊控制器实现对路径切换时的混合跟踪控制；所述监督器包括监督状态量处于直线穿刺和弧线穿刺的判定，以及预测路径切换的判定，模型如式(6)：

式中为k+1时刻的坐标比值和k时刻的坐标比值相比较，当k+1时刻的坐标比值等于k时刻的坐标比值时，直线路径输出为1，当k+1时刻的坐标比值大于k时刻的坐标比值时，弧线路径输出为2；判断k+1时刻的位置和k及k-1时刻作比较来判断路径切换，当原来比值不变，突然k+1时刻的坐标比值变大，直线路径切换弧线路径输出为3；当原来比值不断变大，突然k+1时刻的坐标比值不变，弧线路径切换直线路径输出为4。

作为本发明的进一步限定，所述步骤三中的监督器为识别实际路径和目标路径的穿刺模式，用于判定当前穿刺形式以及监督路径切换情况；所述混合切换模糊控制器采用模糊控制方式对PID预瞄控制器和模型预测控制器的输出量进行模糊处理输出控制的加权系数。

作为本发明的进一步限定，所述步骤三中的混合跟踪控制包括当直线穿刺时采用PID预瞄控制器实现对路径的预瞄跟踪；当弧线穿刺时采用模型预测控制器，在时变预测模型下通过对基于预测误差和控制误差的目标函数最小值求解实现跟踪控制；当穿刺模式切换时监督器根据当前控制器输出的预测误差和控制误差输入给混合切换模糊控制器，之后输出控制器增量加权系数给下一控制器。

本发明采用以上技术方案，与现有技术相比有益效果为：1)本发明采用基于预瞄PID控制和基于模型预测控制的混合跟踪方法，在进行直线穿刺时采用易实现的预瞄PID反馈控制方法，基于电机的编码器获取的进给误差进行反馈控制，实现对直线进给的误差预测补偿；在弧线穿刺时采用基于的模型预测控制方法，提高弧线穿刺时的精度及稳定性；采用基于穿刺形式的混合控制方法，提高了跟踪效率。2)本发明采用监督器进行路径监督，在路径切换时将上一控制器的输出量进行模糊控制输出下一控制器的输出权重，实现稳定切换，通过混合切换模糊控制器，提高了路径切换时的跟踪精度。

附图说明

图1为本发明的流程图。

图2为本发明的***框图。

图3为本发明的运动学模型坐标转换图。

图4为弧线穿刺时预弯柔性探针的弹簧模型。

图5为本发明的混合切换模糊控制器。

图6为模糊控制器的隶属度函数。

具体实施方式

如图1和图2所示的融合PID控制和预测模型模糊切换的轨迹跟踪控制方法，包括以下步骤：

步骤一：在惯性坐标系下建立弧线穿刺时预弯柔性探针针尖运动学模型；

如图3所示，惯性坐标系为以路径组合切线方向为x方向，法线方向为y方向建立的惯性坐标系，y方向的值为挠度w。弧线穿刺时预弯柔性探针针尖运动学模型为基于弹簧理论的针转向模型，建立以相切点为圆心，切向为x轴法向为y轴的针轴坐标系，如图4所示，基于弹簧模型，在竖直截面上，将针杆平均分成i段，将每一段针杆上受到的组织压力F_i，拟合为弹簧弹力与针杆弯曲挠度有关，得F_i＝K_rω(x_i)，K_r为随曲率半径变化的弹簧系数，运动学模型：

ω(x)为切向x处对应的针尖挠度，基于由针芯伸长量引起的曲率变化，得到了与曲率有关的系数矩阵A_nl，阶数为4×l阶。

步骤二：设计路径跟踪混合控制器，包括针对直线路径的PID预瞄控制器和针对弧线路径的模型预测控制器；PID预瞄控制器采用基于编码器电机进给量误差值的PID预测控制；模型预测控制器采用基于进给量误差及穿刺曲率的模型预测控制；

直线穿刺时的PID预瞄控制器采用增量式PID控制方法，控制模型如下：

定义***采样周期为T，k为当前采样时刻；u(k)为k时刻的***输出，Δu(k)为***的输出增量，e(k)为***误差由电机编码器获得作为控制器的输入量；优化系数K_p为比例系数，T_i为积分时间，T_d为微分时间。直线穿刺时的PID预瞄控制器采用增量式PID控制方法，控制增量Δu(k)的确定仅与最近3次的采样值有关，容易通过加权处理获得较好的控制效果。

弧线穿刺时采用模型预测控制器，根据预弯柔性探针针尖运动学模型，转化为状态空间方程，建立如下观测模型：

式中x为路径组合切向进给量，为其对时间求导，w为挠度，/>为其对时间求导，θ为针尖与x方向的夹角，l为针芯伸长量，v为穿刺速度，A_l为伸长量为l时的系数矩阵，得到状态变量ξ＝[x w]^T，输入控制量u＝[l v]^T。

得到离散化的预测增量模型：

x(k+1)＝Ax(k)+Bu(k) (4)

式中x(k)为k时刻的状态量，u(k)为k时刻的输出量，通过预测增量模型在定义的控制时域下进行系数A和B的滚动优化，得到最优的初始状态量及控制量，定义控制时域为5。

建立基于预测误差和控制误差的目标函数，求解预测变化及控制变化的矩阵二范数之和的最小值。

式中E_p为预测时域下得到的位置误差矩阵，阶数为预测时域值；E_u为控制时域下得到的输入误差矩阵，阶数为控制时域乘输入控制量个数；Q和R分别为与E_p和E_u相同的带系数的单位阵，系数分别为5和1。

通过拟牛顿法得到目标函数最小值时的控制量，实现对针尖目标的跟踪控制。

步骤三：设计监督器和混合切换模糊控制器实现对路径切换时的混合跟踪控制。

监督器包括监督状态量处于直线穿刺和弧线穿刺的判定，以及预测路径切换的判定，模型如下：

式中为k+1时刻的坐标比值和k时刻的坐标比值相比较，当k+1时刻的坐标比值等于k时刻的坐标比值时，直线路径输出为1，当k+1时刻的坐标比值大于k时刻的坐标比值时，弧线路径输出为2；判断k+1时刻的位置和k及k-1时刻作比较来判断路径切换，当原来比值不变，突然k+1时刻的坐标比值变大，直线路径切换弧线路径输出为3；当原来比值不断变大，突然k+1时刻的坐标比值不变，弧线路径切换直线路径输出为4。

设计混合切换模糊控制器，如图5所示，将当前控制器输出的预测误差e_p和控制误差e_u作为模糊控制器的输入，输出为控制增量系数λ₁和λ₂，包括进给速度和针芯伸长量，得到稳定的切换控制量。

控制器输入量预测差值和控制差值的论域为[-10,10]，模糊论域为{-2,-1,0,1,2}，对应的模糊子集为{NB,NS,ZO,PS,PB}；模糊集差值变化率的论域为[-3,3]；模糊论域为{-1,0,1}，对应的模糊子集为{N,ZO,P}，N为负，ZO为零，P为正，NB为负大，NS为负小，PS为正小，PB为正大，输入量采用高斯型隶属度函数。

输出量为加权系数，论域为[0,1]，模糊论域为{0,0.5,1}，对应的模糊子集为{ZO,PS,PB}，输出量采用S型和三角型隶属度函数，如图6所示。

定义的控制增量系数λ₁的控制规则表如表1：

定义的控制增量系数λ₂的控制规则表如表2：

简要说明模糊控制规则如下：

当输出差值为正大，且差值变化率为正时，此时偏差较大且逐渐增大，此时输出系数λ₁应为较大，输出系数λ₂应为零；差值变化率为负时，此时偏差较大且逐渐缩小，此时输出系数λ₁应为较大，输出系数λ₂应为较大；差值变化率为零时，此时输出系数λ₁应为较小，输出系数λ₂应较大；当输出差值为负大时，情况与上述相反。

当输出差值为正小时，且差值变化率为正时，此时输出值距目标值很近，差距在逐渐增大，此时输出系数λ₁应为较小，输出系数λ₂应为较小；差值变化率为负时，此时输出值距目标值很近，且逐渐缩小差距，此时输出系数λ₁应为零，输出系数λ₂应为较小；差值变化率为零时，此时输出系数λ₁应为较大，输出系数λ₂应较小；当输出差值为负小时，情况与上述相反。

当输出差值为零，且差值变化率为正时，此时输出值符合要求，却逐渐超前偏离，此时输出系数λ₁应为零，输出系数λ₂应较小；差值变化率为负时，此时输出值符合要求，却逐渐落后，此时输出系数λ₁应为较小，输出系数λ₂应零；差值变化率为零时，此时输出系数λ₁应为零，输出系数λ₂应零。

本发明反模糊算法采用最大隶属度法，方法如下，若有如下表格，

	NS	Z0
			N	PS	PB
Z0	ZO	PS

若差值属于[NS,Z0]，差值变化率属于[N,Z0]，另差值对NS的隶属度为a，对Z0的隶属度为1-a，差值变化率对N的隶属度为b，对Z0的隶属度为1-b，则输出值为：

本发明采用了基于预瞄PID控制和基于的模型预测控制的混合跟踪方法，在进行直线穿刺时采用易实现的预瞄PID反馈控制方法，基于电机的编码器获取的进给误差进行反馈控制，实现对直线进给的预瞄跟踪；在弧线穿刺时采用基于的模型预测控制方法，提高弧线穿刺时的精度；采用模糊控制的方法进行控制器的切换，提高切换的稳定性。

本发明并不局限于上述实施例，在本发明公开的技术方案的基础上，本领域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明的保护范围内。

Claims (3)

1.一种融合PID控制和预测模型模糊切换的轨迹跟踪控制方法，其特征在于，包括以下步骤：

步骤一：在惯性坐标系下建立弧线穿刺时预弯柔性探针针尖运动学模型；

所述步骤一中的惯性坐标系为以路径组合切线方向为x方向，法线方向为y方向建立的惯性坐标系，y方向的值为挠度w；所述弧线穿刺时预弯柔性探针针尖运动学模型为基于弹簧理论的针转向模型，建立以相切点为圆心，切向为x轴法向为y轴的针轴坐标系，运动学模型如式(1)：

式中a_nl为三次多项式系数，对于不同针芯伸长量l，都有相对应的四个系数a_nl，n＝0,1,2,3时，ω(x)为路径组合切向进给量x处对应的针尖挠度，基于由针芯伸长量引起的曲率变化，得到了与曲率有关的系数矩阵A_nl，系数矩阵A_nl由四个系数a_nl，n＝0,1,2,3组成，阶数为4×l阶；

步骤二：设计路径跟踪混合控制器，包括针对直线路径的PID预瞄控制器和针对弧线路径的模型预测控制器；所述PID预瞄控制器采用基于编码器电机进给量误差值的PID预测控制；所述模型预测控制器采用基于进给量误差及穿刺曲率的模型预测控制；

直线穿刺时的PID预瞄控制器采用增量式PID控制方法，控制模型如下：

定义***采样周期为T，k为当前采样时刻；u(k)为k时刻的***输出量，Δu(k)为***的输出增量，e(k)为***误差由电机编码器获得作为控制器的输入量；优化系数K_p为比例系数，T_i为积分时间，T_d为微分时间；

弧线穿刺时采用模型预测控制器，根据预弯柔性探针针尖运动学模型，转化为状态空间方程，建立如式(3)观测模型：

式中x为路径组合切向进给量，为其对时间求导，w为挠度，/>为其对时间求导，θ为针尖与切向的夹角，l为针芯伸长量，v为穿刺速度，A_l为伸长量为l时的系数矩阵，得到状态变量ξ＝[x w]^T，输入控制量u＝[l v]^T；

得到离散化的预测增量模型如式(4)：

x(k+1)＝Ax(k)+Bu(k) (4)

式中x(k)为k时刻的状态量，u(k)为k时刻的***输出量，通过预测增量模型在定义的控制时域下进行系数A和B的滚动优化，得到最优的初始状态量及控制量，定义控制时域为5；

建立基于预测误差和控制误差的目标函数如式(5)，求解预测变化及控制变化的矩阵二范数之和的最小值；

式中E_p为预测时域下得到的位置误差矩阵，阶数为预测时域值；E_u为控制时域下得到的输入误差矩阵，阶数为控制时域乘输入控制量个数；Q和R分别为与E_p和E_u相同阶的带系数的单位阵，系数分别为5和1；

通过拟牛顿法得到目标函数的最小值时的控制量，实现对针尖目标的跟踪控制；

步骤三：设计监督器和混合切换模糊控制器实现对路径切换时的混合跟踪控制；所述监督器包括监督状态量处于直线穿刺和弧线穿刺的判定，以及预测路径切换的判定，模型如式(6)：

式中为k+1时刻的坐标比值和k时刻的坐标比值相比较，当k+1时刻的坐标比值等于k时刻的坐标比值时，直线路径输出为1，当k+1时刻的坐标比值大于k时刻的坐标比值时，弧线路径输出为2；判断k+1时刻的位置和k及k-1时刻作比较来判断路径切换，当原来比值不变，突然k+1时刻的坐标比值变大，直线路径切换弧线路径输出为3；当原来比值不断变大，突然k+1时刻的坐标比值不变，弧线路径切换直线路径输出为4。

2.根据权利要求1所述的融合PID控制和预测模型模糊切换的轨迹跟踪控制方法，其特征在于，所述步骤三中的监督器为识别实际路径和目标路径的穿刺模式，用于判定当前穿刺形式以及监督路径切换情况；所述混合切换模糊控制器采用模糊控制方式对PID预瞄控制器和模型预测控制器的输出量进行模糊处理输出控制的加权系数。

3.根据权利要求1所述的融合PID控制和预测模型模糊切换的轨迹跟踪控制方法，其特征在于，所述步骤三中的混合跟踪控制包括当直线穿刺时采用PID预瞄控制器实现对路径的预瞄跟踪；当弧线穿刺时采用模型预测控制器，在时变预测模型下通过对基于预测误差和控制误差的目标函数最小值求解实现跟踪控制；当穿刺模式切换时监督器根据当前控制器输出的预测误差和控制误差输入给混合切换模糊控制器，之后输出控制器增量加权系数给下一控制器。