CN110262230B - 一种基于mpc算法的柔性针穿刺路径跟踪控制方法 - Google Patents
一种基于mpc算法的柔性针穿刺路径跟踪控制方法 Download PDFInfo
- Publication number
- CN110262230B CN110262230B CN201910455267.2A CN201910455267A CN110262230B CN 110262230 B CN110262230 B CN 110262230B CN 201910455267 A CN201910455267 A CN 201910455267A CN 110262230 B CN110262230 B CN 110262230B
- Authority
- CN
- China
- Prior art keywords
- flexible needle
- needle
- kinematic
- model
- control
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
- Surgical Instruments (AREA)
Abstract
本发明公开了一种基于MPC算法的柔性针穿刺路径跟踪控制方法,属于柔性针***的控制算法领域。首先根据柔性针的运动特性,在惯性坐标系XOY下建立针尖的运动学模型并转化为状态空间方程;然后将柔性针的运动学模型进行线性化及离散化处理,得到MPC中柔性针的运动学线性时变预测模型;将柔性针的运动学模型进行线性化及离散化处理,得到MPC中柔性针的运动学线性时变预测模型;最后利用运动学线性时变预测模型实时在线求解优化目标函数,得到最优的进给速度vr和穿刺曲率K,完成柔性针穿刺路径的跟踪控制。本发明十分适合柔性针这种难以精确建模的运动模型,有助于判断穿刺手术能否继续进行。
Description
技术领域
本发明属于柔性针***的控制算法领域,具体是一种基于MPC算法的柔性针穿刺路径跟踪控制方法。
背景技术
微创医疗是当代甚至未来临床医疗技术的发展重点,这些年对医疗机器人辅助***的研究也逐渐被重视。穿刺手术作为微创手术中最为普遍的一项技术,利用其创伤小见效快的特点,被广泛应用于组织检查和药物投送(靶向注射)等手术中。
传统的穿刺手术使用的是穿刺钢针,由于其自身特点无法对针尖位置进行精确控制,当针尖由于受力不均等原因导致轨迹发生偏离时,只能重新穿刺。因此有研究学者提出柔性针的概念,柔性针具有直径小、针尖非对称斜角和弹性较好等特点,在穿刺时针尖受到组织给予的非对称作用力,会发生偏转,改变穿刺路径。根据此原理,在穿刺过程中通过控制柔性针的进给速度和针尖的斜面朝向,就能实现对穿刺路径和针尖位置的控制。
实际的柔性针控制***中,同样具有两个自由度,一个是针的进给运动,另一个是针轴的自转运动。进给运动实现针尖在组织中的穿刺行进,而自转运动则改变针尖的穿刺方向,根据此特性可以将针尖的运动形式等效成一个空间的独轮车,穿刺路径是一定曲率范围内的曲线。通过对两个运动的协调控制,就可以实现一定形式的空间穿刺路径。
发明内容
为了有效而精确的实现柔性针的路径跟踪控制,本发明提出了一种基于MPC算法的柔性针穿刺路径跟踪控制方法,将MPC算法应用在柔性针***的平面路径控制上,确定柔性针在平面上的运动学模型,结合该模型利用MPC算法的原理:通过预测模型,滚动优化和反馈校正,实现柔性针按照预设的穿刺路径进行精确穿刺。
具体步骤如下:
步骤一、根据柔性针的运动特性,在惯性坐标系XOY下建立针尖的运动学模型并转化为状态空间方程;
运动学模型如下:
运动学模型转换的状态空间形式表示为:
步骤二、将柔性针的运动学模型进行线性化及离散化处理,得到MPC中柔性针的运动学线性时变预测模型;
首先,将柔性针运动学模型简化为:
式中:(x,y)为柔性针针尖坐标,v为柔性针进给速度。
然后,对简化的运动学模型进行线性化处理,得到新的连续的柔性针运动学状态方程;
方程公式如下:
最后,将新的连续的柔性针运动学状态方程进行离散化处理,得到用于MPC的运动学线性时变预测模型;
模型公式如下:
式中:
T为采样时间。
步骤三、对运动学模型中柔性针的进给速度vr的范围和曲率K的变化范围加以约束,建立以穿刺路径跟踪效果为核心的优化目标函数;
目标函数为:
Np为预测时域,Q为状态量偏差权重矩阵,R为控制量偏差权重矩阵;Q>0,R>0。
柔性针***的控制量极限约束:
umin(k+t)≤u(k+t)≤umax(k+t)
t=0,1,…,Nc-1
在带约束求解的过程中,当无可行解时,令当前控制量u(k|k)=u(k|k-1),表示采样时刻k的第1个值取采样时刻k-1求解得到的第2个值。
步骤四、利用运动学线性时变预测模型实时在线求解优化目标函数,得到最优的进给速度vr和穿刺曲率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下计算针尖的进给速度:
然后,建立针尖运动学约束:
联立式(1)和式(2)得:
利用图示运动关系,解得柔性针角速度ω:
ρ=f(w) (5)
结合式(3)和式(4)得到针尖的运动学模型如下:
将该模型转换为更一般的状态空间形式表示:
步骤二、对柔性针的非线性运动学模型进行线性化及离散化处理,得到能用于MPC控制的线性时变预测模型;
本发明采用存在参考***的线性化方法,首先将柔性针运动学模型(6)简化为:
式中:(x,y)为柔性针针尖坐标,v为柔性针进给速度。
此时,将柔性针穿刺的期望路径作为参考***,柔性针于参考轨迹上任意时刻(或任意工作点)的状态与控制量均满足:
对在任意工作点(ξr,ur)处进行泰勒展开,仅保留一阶项,简化计算得到:
将式(10)与式(9)相减,可以得到:
由此,得到了新的柔性针运动学状态方程,但该状态方程是连续的,无法直接应用到柔性针的控制器设计中,因此还需对参考***进行离散化处理:
Akin(k)=I+TA(k) (12)
Bkin(k)=TB(k) (13)
其中,T为采样时间;将式(12)和式(13)结合式(11),得到:
式中:
此时就完成了参考轨迹和***的离散化处理,下一步进行MPC控制器的设计,使柔性针***能够完成对轨迹的跟踪。
步骤三、设定合适的优化目标函数,并确定MPC控制器的约束条件;
目标函数是为了保证柔性针在控制的过程中,能够快速且平稳的追踪期望轨迹,因此需要加入对***状态量的偏差和控制量的优化。
本发明采用的是下面这种形式的目标函数:
式中,Np为预测时域,Q为状态量偏差权重矩阵,R为控制量偏差权重矩阵;Q>0,R>0。
对于这种形式的优化目标函数,利用适当的变换处理,将其转变为常见的二次规划问题,优化的目标变为二次实函数,可以带有线性或者非线性的约束条件。
处理方式如下:令柔性针***方程为:
其中Np为预测时域,表明优化的整段时域长度;Nc为控制时域,表明Nc时域后的控制量保持不变。因此,式(15)改写为:
其中:
因此,式(16)可以写成:
其中:
结合式(18)和式(17),将原目标函数(15)变换成:
其中:
由于柔性针的针尖斜面参数确定,因此其穿刺运动过程中路径的最大曲率(±Kmax)已经确定,改变自转电机的占空比可以减小穿刺路径的曲率至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)
以此控制序列作为输入的柔性针***,即可完成对期望穿刺路径的跟踪。
最后利用软件仿真验证。
Claims (1)
1.一种基于MPC算法的柔性针穿刺路径跟踪控制方法,其特征在于,具体步骤如下:
步骤一、根据柔性针的运动特性,在惯性坐标系XOY下建立针尖的运动学模型并转化为状态空间方程;
运动学模型如下:
运动学模型转换的状态空间形式表示为:
步骤二、将柔性针的运动学模型进行线性化及离散化处理,得到MPC中柔性针的运动学线性时变预测模型;
具体为:
首先,将柔性针运动学模型简化为:
式中:(x,y)为柔性针针尖坐标,v为柔性针进给速度;
然后,对简化的运动学模型进行线性化处理,得到新的连续的柔性针运动学状态方程;
方程公式如下:
最后,将新的连续的柔性针运动学状态方程进行离散化处理,得到用于MPC的运动学线性时变预测模型;
模型公式如下:
式中:
T为采样时间;
步骤三、对运动学模型中柔性针的进给速度vr的范围和曲率K的变化范围加以约束,建立以穿刺路径跟踪效果为核心的优化目标函数;
目标函数为:
Np为预测时域,Q为状态量偏差权重矩阵,R为控制量偏差权重矩阵;Q>0,R>0;
柔性针***的控制量极限约束:
umin(k+t)≤u(k+t)≤umax(k+t)
t=0,1,…,Nc-1
Nc为控制时域,表明Nc时域后的控制量保持不变;
在带约束求解的过程中,当无可行解时,令当前控制量u(k|k)=u(k|k-1),表示采样时刻k的第1个值取采样时刻k-1求解得到的第2个值;
步骤四、利用运动学线性时变预测模型实时在线求解优化目标函数,得到最优的进给速度vr和穿刺曲率K,完成柔性针穿刺路径的跟踪控制;
最终的全局控制序列为:
{u(k|k),u(k+1|k+1),...,u(∞|∞)}
以此控制序列作为输入的柔性针***,即可完成对期望穿刺路径的跟踪。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910455267.2A CN110262230B (zh) | 2019-05-29 | 2019-05-29 | 一种基于mpc算法的柔性针穿刺路径跟踪控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910455267.2A CN110262230B (zh) | 2019-05-29 | 2019-05-29 | 一种基于mpc算法的柔性针穿刺路径跟踪控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110262230A CN110262230A (zh) | 2019-09-20 |
CN110262230B true CN110262230B (zh) | 2020-08-11 |
Family
ID=67915821
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910455267.2A Active CN110262230B (zh) | 2019-05-29 | 2019-05-29 | 一种基于mpc算法的柔性针穿刺路径跟踪控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110262230B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111048212B (zh) * | 2019-12-20 | 2023-04-18 | 华中科技大学 | 基于深度强化学习的斜尖柔性针路径跟踪用网络优化方法 |
CN111916214B (zh) * | 2020-07-16 | 2024-04-16 | 深圳爱博合创医疗机器人有限公司 | 一种用于血管介入手术医生训练***的导管轨迹预测方法 |
CN113867133B (zh) * | 2021-11-03 | 2024-04-16 | 扬州大学江都高端装备工程技术研究所 | 融合pid控制和预测模型模糊切换的轨迹跟踪控制方法 |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140018788A1 (en) * | 2012-07-04 | 2014-01-16 | Zoar Jacob ENGELMAN | Devices and Systems for Carotid Body Ablation |
CN102920510A (zh) * | 2012-10-29 | 2013-02-13 | 清华大学 | 柔性穿刺针的路径规划方法 |
CN104248471B (zh) * | 2013-06-27 | 2016-09-07 | 中国科学院沈阳自动化研究所 | 一种机器人辅助斜尖柔性针穿刺***及方法 |
CN105262083B (zh) * | 2015-10-21 | 2019-05-21 | 中国电力科学研究院 | 一种基于时变负荷扰动的mpc二次调频有功分配方法 |
CN105631864B (zh) * | 2015-12-23 | 2018-06-12 | 电子科技大学 | 一种基于时空相关性的心脏表面目标点运动预测方法 |
CN105955023B (zh) * | 2016-05-16 | 2019-02-19 | 浙江大学 | 一种用于快速运动控制***的模型预测控制方法 |
US20180368883A1 (en) * | 2017-05-17 | 2018-12-27 | The Governors Of The University Of Alberta | Hand-held device and computer-implemented system and method for assisted steering of a percutaneously inserted needle |
US10386726B2 (en) * | 2017-09-29 | 2019-08-20 | Globalfoundries Inc. | Geometry vectorization for mask process correction |
CN107808698B (zh) * | 2017-11-08 | 2021-10-15 | 哈尔滨理工大学 | 一种柔性针穿刺软组织的力学及弯曲变形建模方法 |
-
2019
- 2019-05-29 CN CN201910455267.2A patent/CN110262230B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110262230A (zh) | 2019-09-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110262230B (zh) | 一种基于mpc算法的柔性针穿刺路径跟踪控制方法 | |
US11571258B2 (en) | Path planning method with artificial potential field based on obstacle classification and medical system for steering flexible needle | |
CN109171965B (zh) | 基于虚拟夹具的机器人的控制方法及*** | |
CN107053179B (zh) | 一种基于模糊强化学习的机械臂柔顺力控制方法 | |
Wang et al. | A hybrid visual servo controller for robust grasping by wheeled mobile robots | |
CN103901898A (zh) | 一种多自由度机器人的逆运动学通用求解方法 | |
CN104933232B (zh) | 一种杆端浮动型六自由度并联机器人带角度补偿的运动学求解方法 | |
CN113721607B (zh) | 轮式机器人轨迹跟踪最优控制方法 | |
Yang et al. | Smooth trajectory planning along Bezier curve for mobile robots with velocity constraints | |
CN112618346B (zh) | 一种基于多传感器感知的艾灸机器人运动控制方法 | |
CN111650831A (zh) | 对医疗机器人控制器中虚拟柔性针的区间2型模糊逻辑控制器的设计 | |
CN107807522A (zh) | 水下机器人轨迹跟踪反步控制方法 | |
CN111515938B (zh) | 一种继承型迭代学习控制的下肢外骨骼行走轨迹跟踪方法 | |
Qinpeng et al. | Path tracking control of wheeled mobile robot based on improved pure pursuit algorithm | |
Zidane et al. | Trajectory control of a laparoscopic 3-PUU parallel manipulator based on neural network in SIMSCAPE SIMULINK environment | |
CN113568422B (zh) | 基于模型预测控制优化强化学习的四足机器人控制方法 | |
CN109875659B (zh) | 基于大脑情感学***面穿刺控制装置及方法 | |
Sun et al. | Visual‐based autonomous field of view control of laparoscope with safety‐RCM constraints for semi‐autonomous surgery | |
CN116619369A (zh) | 一种基于遥操作柔性机械臂的共享控制方法及其应用 | |
Zhu et al. | State feedback fault tolerant control for flexible bevel-tip needle based on proportional multiple-integral observer | |
Wang et al. | Navigation control of mobile robot based on fuzzy neural network | |
CN113867133B (zh) | 融合pid控制和预测模型模糊切换的轨迹跟踪控制方法 | |
Sasiadek et al. | Path tracking of an autonomous LHD articulated vehicle | |
Zakaria et al. | Forward and inverse predictive model for the trajectory tracking control of a lower limb exoskeleton for gait rehabilitation: Simulation modelling analysis | |
CN117922548B (zh) | 一种基于模型预测控制的自动泊车轨迹规划方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |