一种基于MPC算法的柔性针穿刺路径跟踪控制方法

摘要

本发明公开了一种基于MPC算法的柔性针穿刺路径跟踪控制方法，属于柔性针系统的控制算法领域。首先根据柔性针的运动特性，在惯性坐标系XOY下建立针尖的运动学模型并转化为状态空间方程；然后将柔性针的运动学模型进行线性化及离散化处理，得到MPC中柔性针的运动学线性时变预测模型；将柔性针的运动学模型进行线性化及离散化处理，得到MPC中柔性针的运动学线性时变预测模型；最后利用运动学线性时变预测模型实时在线求解优化目标函数，得到最优的进给速度v

说明书

技术领域

本发明属于柔性针系统的控制算法领域，具体是一种基于MPC算法的柔性针穿刺路径跟踪控制方法。

背景技术

微创医疗是当代甚至未来临床医疗技术的发展重点，这些年对医疗机器人辅助系统的研究也逐渐被重视。穿刺手术作为微创手术中最为普遍的一项技术，利用其创伤小见效快的特点，被广泛应用于组织检查和药物投送(靶向注射)等手术中。

传统的穿刺手术使用的是穿刺钢针，由于其自身特点无法对针尖位置进行精确控制，当针尖由于受力不均等原因导致轨迹发生偏离时，只能重新穿刺。因此有研究学者提出柔性针的概念，柔性针具有直径小、针尖非对称斜角和弹性较好等特点，在穿刺时针尖受到组织给予的非对称作用力，会发生偏转，改变穿刺路径。根据此原理，在穿刺过程中通过控制柔性针的进给速度和针尖的斜面朝向，就能实现对穿刺路径和针尖位置的控制。

实际的柔性针控制系统中，同样具有两个自由度，一个是针的进给运动，另一个是针轴的自转运动。进给运动实现针尖在组织中的穿刺行进，而自转运动则改变针尖的穿刺方向，根据此特性可以将针尖的运动形式等效成一个空间的独轮车，穿刺路径是一定曲率范围内的曲线。通过对两个运动的协调控制，就可以实现一定形式的空间穿刺路径。

发明内容

为了有效而精确的实现柔性针的路径跟踪控制，本发明提出了一种基于MPC算法的柔性针穿刺路径跟踪控制方法，将MPC算法应用在柔性针系统的平面路径控制上，确定柔性针在平面上的运动学模型，结合该模型利用MPC算法的原理：通过预测模型，滚动优化和反馈校正，实现柔性针按照预设的穿刺路径进行精确穿刺。

具体步骤如下：

步骤一、根据柔性针的运动特性，在惯性坐标系XOY下建立针尖的运动学模型并转化为状态空间方程；

运动学模型如下：

(X_r,Y_r)为针尖位置所处坐标；v_r为针尖(X_r,Y_r)处的进给速度；为柔性针航向角，为针尖当前进给方向与惯性坐标系XOY下X轴的夹角；K为针尖运动的曲率。

运动学模型转换的状态空间形式表示为：

其中，状态变量控制量u_c＝[v_r>T。

步骤二、将柔性针的运动学模型进行线性化及离散化处理，得到MPC中柔性针的运动学线性时变预测模型；

首先，将柔性针运动学模型简化为：

式中：(x,y)为柔性针针尖坐标，v为柔性针进给速度。

然后，对简化的运动学模型进行线性化处理，得到新的连续的柔性针运动学状态方程；

方程公式如下：

k为采样时刻，为f相对于系统状态变量ξ的雅可比矩阵，为f相对于控制量u的雅可比矩阵,为系统状态量与状态期望值的偏差；为控制量与控制期望值的偏差。

最后，将新的连续的柔性针运动学状态方程进行离散化处理，得到用于MPC的运动学线性时变预测模型；

模型公式如下：

式中：

T为采样时间。

步骤三、对运动学模型中柔性针的进给速度v_r的范围和曲率K的变化范围加以约束，建立以穿刺路径跟踪效果为核心的优化目标函数；

目标函数为：

N_p为预测时域，Q为状态量偏差权重矩阵，R为控制量偏差权重矩阵；Q>0，R>0。

柔性针系统的控制量极限约束：

u_min(k+t)≤u(k+t)≤u_max(k+t)

t＝0,1,…,N_c-1

在带约束求解的过程中，当无可行解时，令当前控制量u(k|k)＝u(k|k-1)，表示采样时刻k的第1个值取采样时刻k-1求解得到的第2个值。

步骤四、利用运动学线性时变预测模型实时在线求解优化目标函数，得到最优的进给速度v_r和穿刺曲率K，完成柔性针穿刺路径的跟踪控制。

最终的全局控制序列为：

{u(k|k),u(k+1|k+1),...,u(∞|∞)}

以此控制序列作为输入的柔性针系统，即可完成对期望穿刺路径的跟踪。

本发明的优点在于：

一种基于MPC算法的柔性针穿刺路径跟踪控制方法，相比其他的控制算法，MPC的鲁棒性更强，由于其实时在线滚动优化的特性，对模型的精度要求不高，十分适合柔性针这种难以精确建模的运动模型。若能与位置观测类传感器相配合，感知穿刺过程中遇到组织变化等外界因素导致的柔性针位置状态变化，就能通过MPC算法提前预测未来穿刺状态，有助于判断穿刺手术能否继续进行。

附图说明

图1为本发明利用MPC算法进行穿刺路径跟踪的原理图。

图2为本发明一种基于MPC算法的柔性针穿刺路径跟踪控制方法流程图；

图3为本发明在惯性坐标系XOY下建立针尖的柔性针运动学模型图。

图4为本发明MPC算法的控制原理图。

具体实施方式

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

本发明涉及一种基于模型预测控制(MPC)算法在柔性针系统上的控制器设计，通过建立柔性针系统的运动学模型旨在完成柔性针系统的穿刺路径跟踪控制；如图1所示，首先建立柔性针参考轨迹，并对参考轨迹进行离散化，依次选择轨迹上的点，利用MPC控制器进行跟踪，直至将轨迹上的点全部跟踪完毕，由于MPC算法对模型的精度要求不高，十分适合应用在柔性针这种难以精确建模的运动模型上。

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

步骤一、根据柔性针的运动特性，针对其运动几何规律建立状态空间模型。

假设其针尖斜面的法线始终在同一平面上，从而使其穿刺运动路径维持在同一平面，通过改变自转电机的占空比改变穿刺路径曲率。

如图3所示，首先在惯性坐标系XOY下计算针尖的进给速度：

其中(X_r,Y_r)为针尖位置所处坐标；v_r为针尖(X_r,Y_r)处的进给速度；为柔性针航向角，为针尖当前进给方向与惯性坐标系XOY下X轴的夹角。

然后，建立针尖运动学约束：

联立式(1)和式(2)得：

利用图示运动关系，解得柔性针角速度ω：

式中为下一时刻柔性针的针尖进给方向与X轴的夹角，Δt为两次时间间隔，θ为绕运动圆心转过的角度，K为针尖运动的曲率，ρ为曲率半径，由自转电机的占空比w确定:

ρ＝f(w)(5)

结合式(3)和式(4)得到针尖的运动学模型如下：

将该模型转换为更一般的状态空间形式表示：

其中，状态变量控制量u_c＝[v_r>T。

步骤二、对柔性针的非线性运动学模型进行线性化及离散化处理，得到能用于MPC控制的线性时变预测模型；

本发明采用存在参考系统的线性化方法，首先将柔性针运动学模型(6)简化为：

式中：(x,y)为柔性针针尖坐标，v为柔性针进给速度。

此时，将柔性针穿刺的期望路径作为参考系统，柔性针于参考轨迹上任意时刻(或任意工作点)的状态与控制量均满足：

对在任意工作点(ξ_r,u_r)处进行泰勒展开，仅保留一阶项，简化计算得到：

将式(10)与式(9)相减，可以得到：

式中k为采样时刻，为f相对于系统状态变量ξ的雅可比矩阵，为f相对于控制量u的雅可比矩阵,为系统状态量与状态期望值的偏差；为控制量与控制期望值的偏差。

由此，得到了新的柔性针运动学状态方程，但该状态方程是连续的，无法直接应用到柔性针的控制器设计中，因此还需对参考系统进行离散化处理：

A_kin(k)＝I+TA(k)(12)

B_kin(k)＝TB(k)(13)

其中，T为采样时间；将式(12)和式(13)结合式(11)，得到：

式中：

此时就完成了参考轨迹和系统的离散化处理，下一步进行MPC控制器的设计，使柔性针系统能够完成对轨迹的跟踪。

步骤三、设定合适的优化目标函数，并确定MPC控制器的约束条件；

目标函数是为了保证柔性针在控制的过程中，能够快速且平稳的追踪期望轨迹，因此需要加入对系统状态量的偏差和控制量的优化。

本发明采用的是下面这种形式的目标函数：

式中，N_p为预测时域，Q为状态量偏差权重矩阵，R为控制量偏差权重矩阵；Q>0，R>0。

对于这种形式的优化目标函数，利用适当的变换处理，将其转变为常见的二次规划问题，优化的目标变为二次实函数，可以带有线性或者非线性的约束条件。

处理方式如下：令柔性针系统方程为：

取：

其中N_p为预测时域，表明优化的整段时域长度；N_c为控制时域，表明N_c时域后的控制量保持不变。因此，式(15)改写为：

其中：

其中均为常数，表明近处路径的误差控制优先级更高。同时权重Q和R的值也能根据当前的误差控制情况进行调节，误差较大时以控制路径误差为主，即增大权重Q的取值。

因此，式(16)可以写成：

其中：

结合式(18)和式(17)，将原目标函数(15)变换成：

其中：

由于柔性针的针尖斜面参数确定，因此其穿刺运动过程中路径的最大曲率(±K_max)已经确定，改变自转电机的占空比可以减小穿刺路径的曲率至0(直线穿刺)。因此在控制过程中需要对曲率变量K的取值范围加以限制。

同时在柔性针穿刺过程中，为患者的安全考虑，针的进给速度变化范围也不宜太大。综合以上观点，该发明主要考虑柔性针系统的控制量极限约束；

在带约束求解的过程中，若出现无可行解的情况，则令当前控制量u(k|k)＝u(k|k-1)，表示采样时刻k的第1个值取采样时刻k-1求解得到的第2个值。

步骤四、利用设计好的MPC控制器，求解目标函数得到柔性针系统的跟踪控制序列；

MPC是根据系统预测模型、滚动优化以及反馈校正三个基本原理提出的闭环最优控制算法，如图4所示，建立了系统的预测模型之后，在满足系统约束条件的前提下，对涉及系统未来状态量与控制量的性能指标(目标函数)进行优化求解，由此确定系统下一采样时刻的控制行为，将这一控制量输入到控制系统中。之后控制系统会将下一时刻的状态量再次输入到MPC控制器中，如此往复循环实现滚动优化控制。

结合前三步中的数学公式进行描述：在当前采样时刻k，若控制优化问题式(15)符合实际约束条件式(20)，且有可行解，那么就能获得预测时域N内的一系列系统最优控制输入量，记作u*(k)＝{u(k|k),u(k+1|k),...,u(k+N-1|k)}，与之相对应的最优目标函数记作而在求解得到的控制序列u*(k)中，只选择第一个控制量u(k|k)作为系统输入值，保持到下一个控制周期的到来。到了采样时刻k+t时，由于柔性针系统的状态量改变为式(15)构成的优化问题也随之刷新，再一次对其进行求解将得到控制序列u*(k+t)，此时选取的系统输入值为u(k+t|k+t)。

由此可得，经过MPC控制器优化求解的柔性针控制系统，最终的全局控制序列为：

{u(k|k),u(k+1|k+1),...,u(∞|∞)}(21)

以此控制序列作为输入的柔性针系统，即可完成对期望穿刺路径的跟踪。

最后利用软件仿真验证。