CN111904486B 螺旋型血管机器人姿轨一体化自适应滑模跟踪控制方法 （吉林大学）

号螺旋型血管机器人姿轨一体化自适应滑模一种螺旋型血管机器人姿轨一体化自适应的目的是充分考虑血管机器人的姿轨耦合和重力-浮力的影响，且提高控制精度的螺旋型血管对竖直方向重力-浮力的下沉作用进行补偿；设计基于重力-浮力补偿的滑模控制器；基于步骤2步骤一：采用对偶四元数对螺旋型血管机器人的姿态和单位对偶四元数描述的血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的姿轨误差i.为：i.-i;xi,(1)i,和i,分别为利用单位对偶四元数表示的血管机器人期望坐标系OdXdYdZd和本体血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的速度旋量误差在坐标系o;为血管机器人本体坐标系ObXbYbZb相对于惯性坐标系OXYZ的速度旋量在机器人本体坐标系ObXbYbZb中的表示，o;为血管机器人期望坐标系OdXdYdZd相对于惯性坐标系进而通过推导可以得到基于i,,:的姿轨一体化运动学及动力学方程：b为对偶质量特性惯性矩阵，表示坐标系ObXbYbZb中作用点为血管机器人质心3机器人主要受到总驱动力机器人的重力-浮力.,流体阻力由血管机器人头部和螺旋尾部受血流冲击所产生的流体阻力组成，f其中为设计重力-浮p轴与血管机器人实际指向相重合，z轴垂直于血管机器人实际指向为血管机p将血管机器人期望运动速度分解为水平分量和竖直分量可知血管机器人期望运动速度的水平分量与的水平分量相等，即有血管机器度和三维旋转磁场的旋转角速度Ω1有如下关系：4西=Q.=2,(10)当时，将分解为两p面上且方向与血管机器人期望运动速度关于x轴对称，则类似于的受力分析，可p以分解为平行于x轴的分量与垂直于x轴的分量其中，和分别表示在z轴与x轴投影的数量值，和在z5通过分析血管机器人姿态坐标系p的力学几根据cos(θ+β)＝cos(θ)cos(β)-sin(θ)sin(β)，则由式(18)可得重力-浮力补偿器输出度Ω16动描述i,,从而将血管机器人期望坐标系的对偶四元数运动i,作为控制系OdXdYdZd的姿轨误差i,使血管机器人的实际运动状态(i,0),a,()渐进收敛到期望坐标系基于步骤一中建立的血管机器人姿轨一体化运动学和动力学方程其中，…为对偶数的积运算，为控制器参数，kεi＞0k,-0,"为7进一步得到为三维对偶向量；由于对偶质量特性惯性矩阵Mb的特殊性可y)a。(29)δ212量与惯量往往具有不确定性与多变性，因此为对偶质量特性惯性矩阵和对偶干扰力8因而对血管机器人的研究逐渐演变为医学界的重要研竖直方向重力-浮力的影响，血管机器人往往会发生相对于期望运动轨迹而下沉偏移的现[0005]本发明的目的是充分考虑血管机器人的姿轨耦合和重力-浮力的影响，且提高控[0007]步骤一：采用对偶四元数对螺旋型血管机器人的姿态和建立螺旋型血管机器人的姿轨一体化运动学和动9[0009]步骤三：基于步骤一建立的螺旋型血管机器人模型和步骤二设计的重力-浮力补[0012]单位对偶四元数描述的血管机器人本体坐标系ObXbYbZb相对于期望i,分别为利用单位对偶四元数表示的血管机器人期望坐标系OdXdYdZd和[0015]血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的速度旋量误差在[0017]其中，为血管机器人本体坐标系ObXbYbZb相对于惯性坐标系OXYZ的速度旋量在i,"s"表示矩阵和对偶四元数的一种[0023]当血管机器人在血液中运动时，定义血管机器人的期望运动速度为v,在血液中[0026]其中，表示在三维旋转磁场作用下血管机器人螺旋尾部旋转产生的驱动力，i,i组[0034]其中为设计重pp[0037]将血管机器人期望运动速度分解为水平分量和竖直分量可知血管机器人期望运动速度的水平分量与的水平分量相等，即有血管机器人旋转角速度和三维旋转磁场的旋转角速度Ω1有如下关系：平面上且方向与血管机器人期望运动速度关于x轴对称，则类似于的受力分析，[0046]其中，和分别表示在z轴与xp轴与xp轴投影的数量值，',,'E,分别表示zp轴方向与xp轴方向的单位[0048]由于定义血管机器人期望俯仰角为将速度投影到xpp输出的血管机器人相比于理想俯仰角向上倾角速度Ω1为：力-浮力补偿后的姿态角进行对偶四元数变换，得到对偶四元数框架下的运动描述i,,从而将血管机器人期望坐标系的对偶四元数运[0070]血管机器人的控制目标为：通过控制血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的姿轨误差i,使血管机器人的实际运动状态(ic)ho,(o)渐进收敛到期望[0071]基于步骤一中建立的血管机器人姿轨一体化运动学和动力学方且kεi＞0ε2＝0可以进一步得到为三维对偶向量；由于对偶质量特性惯性矩阵Mb的特殊y)a。(29)δ212[0084]由于血管机器人复杂的工作环境及多变的工作内容，在的估计值设计参数自适应律如下：且：耦合从而影响血管机器人使用的问题。考虑到血管机器人在竖直方向受到重力-浮力的作[0093]图2是血管机器人由于重力-浮力影响进行下沉偏移及进行重力-浮力补偿后的示[0119]单位对偶四元数描述的血管机器人本体坐标系ObXbYbZb相对于期望的姿轨误差i.为：i,和i,分别为利用单位对偶四元数表示的血管机器人期望坐标系OdXdYdZd和[0122]血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的速度旋量误差在[0123]=-⃞xdfxi.(2)[0124]其中，o;为血管机器人本体坐标系ObXbYbZb相对于惯性坐标系OXYZ的速度旋量在运算。对于对偶四元数i,定义oi的运算为ε表示对偶单位；定义i(n.)的[0130]当血管机器人在血液中运动时，定义血管机器人的期望运动速度为v。在血液中的血管机器人主要受到总驱动力机器人的重力-浮力流体阻力的作用。在竖直[0133]其中，表示在三维旋转磁场作用下血管机器人螺旋尾部旋转产生的驱动力，[0134]流体阻力由血管机器人头部和螺旋尾部受血流冲击所产生的流体阻力[0139]通过对血管机器人的受力分析可知，如果要抵消重力-浮力对血管机器人的作用pp[0146]将血管机器人期望运动速度分解为水平分量和竖直分量可知血管机器[0147]血管机器人旋转角速度和三维旋转磁场的旋转角速度Ω1有如下关系：量(f丄p在,方向没有分量)。又由于磁场梯度方向的不确定性和多变性，假设作用可以分解为平行于xp轴的分量与垂直于xp轴的分量(f丄p_1在,方向没有分[0156]其中，和分别表示在z轴与x[0159]由于定义血管机器人期望俯仰角为将速度投影到xpp输出的血管机器人相比于理想俯仰角向上倾角速度Ω1为：力-浮力补偿后的姿态角进行对偶四元数变换，得到对偶四元数框架[0181]血管机器人的的控制目标为：通过控制血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的姿轨误差i,使血管机器人的实际运动状态渐进收敛到期[0182]基于步骤1中建立的血管机器人姿轨一体化运动学和动力学方程式(3)和式(4)，符号函数，且sgn($)=sgn(s,)+esgn(5):为控制器参数，且kεi＞0[0189]步骤三设计的滑模控制器存在抖动现象，并且血管机器人在临床医疗应用过程法估计的变化特性及不确定性，(主要体现在血管机器人中对偶质量特性惯性矩阵Mb的不准确性及对偶干扰力的不确定性)，因此为了解决模型参数不确定与不准确问题并抑制可以进一步得到为三维对偶向量；由于对偶质量特性惯性矩阵Mb的特殊y)a,(29)δ212[0197]由于血管机器人复杂的工作环境及多变的工作内容，在的估计值设计参数自适应律如下：v1v2v3ε≠0。a,i均为对偶数；μ为一实数。i-u,+i,其中区,eR,i,定位矢量时(即实数部分的矢量与选取的参考点无关，而对偶部分的矢量与选取的参考点=mig+s0(1016)[0256]对偶四元数是各元素皆为对偶数的四元数或由四元数作为实数部分与对偶部分[0265]定义血管机器人本体坐标系为ObXbYbZb，惯性坐标系为OXYZ，期望坐标系为[0267]由Euler定理可得，当一个刚体绕定点作旋转运动时可理解为该刚体绕通过定点数表示的血管机器人本体坐标系ObXbYbZb相对于惯性坐标系OXYZ的姿态坐标系OXYZ的角速度在机器人本体坐标系ObXbYbZb和惯性坐标系OXY[0273]同理可得用单位四元数表示期望坐标系OdXdYdZd相对于惯性坐标系OXYZ的姿态运[0274]2i,=l,xof;-,xl,[0275]其中，分别为血管机器人坐标系OXYZ的角速度在期望坐标系OdXdYdZd和惯性坐标系OXY[0276]由于单位四元数具有乘性误差性质，则血管机器人本体坐标系ObXbYbZb相对于期[0278]血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的角速o:在坐标系ObXbYbZb下表示为：o"-o;-o;(1024)坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的姿态误差运[0283]其中，为血管机器人本体坐标系ObXbYbZb相对于期望作标系Od个数学表达式中进行表示，因此采用对偶四元数描述血管机器人的三维旋转和平移运动根据式(6)可得a"-a(o",Im),进一步得到：[0296]结合式(1021)和式(1029)可得，基于单位对偶四元数的血管机器人本体坐标系ObXbYbZb相对于惯性坐标系OXYZ的运动学方程即惯性坐标系下血管机器人的姿轨一体化运度旋量在机器人本体坐标系ObXbYbZb和惯性坐标系OXYZ系OXYZ相对于期望坐标系OdXdYdZ度旋量在期望坐标系OdXdYdZd和惯性坐标系OXYZ系ObXbYbZb相对于期望坐标系OdXdY[0304]根据式(1025)和式(1029)可得对偶四元数框架下的血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的姿轨一体旋量误差在坐标系ObXbYbZb和坐标系OdXdYdZd下[0310]其中，序=fter,表示坐标系ObXbYbZf=[0,,表示在坐标系ObXbYbZb下的作用点在血管机器人质心上的[0319]基于式(1024)中用四元数表示的角速度误差，可得用对偶四元数表示的坐标系ObXbYbZb下的血管机器人本体坐标系ObXbYbZb相对于血管机器人期望坐标系OdXdYdZd的速度器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的姿轨一体化器人下沉偏移及进行重力-浮力补偿后的示意图如机器人实际指向为血管机器人期望运动速度，为血管机器人为抵消重力-浮力影响i和竖直分量易[0350]血管机器人旋转角速度和三维旋转磁场的旋转角速度Ω1有如下关系：F=Q,⃞,(1050)或(10)[0359]其中，[0365]由于定义图4中血管机器人期望俯仰角为将速度投影的旋转角速度Ω1为：力-浮力补偿后的姿态角进行对偶四元数变换，得到对偶四元数框架下的运动描述i,从而将血管机器人期望坐标系的对偶四元数运动i,作为控制系统的输入。[0388]血管机器人的的控制目标为：通过控制血管机器人本体坐标系ObXbYbZb相对于期望坐标系OdXdYdZd的姿轨误差i,使血管机器人的实际运动状态(4.(),a,()渐进收敛到期[0389]基于步骤1中建立的血管机器人姿轨一体化运动学和动力学方程式(1041)和式关于"0"的运算见式(1013)。明控制系统在任何初始状态下都能收敛至$=i;再进一步证明闭环控制系统在滑模面$-i上的稳定性。计的变化特性及不确定性，(主要体现在血管机器人中对偶质量特性惯性矩阵Mb的不准确性及对偶干扰力的不确定性)，因此为了解决模型参数不确定与不准确问题并抑制滑模，iesr通过对偶极子运算的特殊性质ε2=δ212[0435]由于血管机器人复杂的工作环境及多变的工作内容，在的估计值设计参数自适应律如下：且：[0439]将式(1084)和(1085)带入式(1068)的滑模控制律，则可得到自适应滑模控制律[0464]实施例一：用步骤4设计的自适应滑模控制器对血管机器人的直线型运动轨迹进[0478]实施例二：用步骤4设计的自适应滑模控制器对血管机器人的曲线型运动轨迹进