[发明专利]螺旋型血管机器人姿轨一体化自适应滑模跟踪控制方法

权利要求书

1.一种螺旋型血管机器人姿轨一体化自适应滑模跟踪控制方法，其特征在于：其步骤是：

步骤一：采用对偶四元数对螺旋型血管机器人的姿态和轨迹运动同时进行描述，建立螺旋型血管机器人的姿轨一体化运动学和动力学模型；

步骤二：针对螺旋型血管机器人在血液中运动时，会受到重力-浮力的影响，导致实际运动轨迹下沉偏移，而脱离期望轨迹，设计重力-浮力补偿器，对竖直方向重力-浮力的下沉作用进行补偿；

步骤三：基于步骤一建立的螺旋型血管机器人模型和步骤二设计的重力-浮力补偿器，设计基于重力-浮力补偿的滑模控制器；

步骤四：基于步骤三设计的滑模控制器，设计自适应滑模控制；

步骤一所述的姿轨一体化运动学和动力学模型建立的具体过程为：

单位对偶四元数描述的血管机器人本体坐标系O_bX_bY_bZ_b相对于期望坐标系O_dX_dY_dZ_d的姿轨误差为：

其中，和分别为利用单位对偶四元数表示的血管机器人期望坐标系O_dX_dY_dZ_d和本体坐标系O_bX_bY_bZ_b分别相对于惯性坐标系OXYZ的姿轨，“*”表示对偶四元数的乘法，表示对偶四元数的共轭；

血管机器人本体坐标系O_bX_bY_bZ_b相对于期望坐标系O_dX_dY_dZ_d的速度旋量误差在坐标系O_bX_bY_bZ_b下表示为：

其中，为血管机器人本体坐标系O_bX_bY_bZ_b相对于惯性坐标系OXYZ的速度旋量在机器人本体坐标系O_bX_bY_bZ_b中的表示，为血管机器人期望坐标系O_dX_dY_dZ_d相对于惯性坐标系OXYZ的速度旋量在期望坐标系O_dX_dY_dZ_d中的表示，(·)^*表示对偶四元数的共轭；

进而通过推导可以得到基于的姿轨一体化运动学及动力学方程：

其中，M^b为对偶质量特性惯性矩阵，表示坐标系O_bX_bY_bZ_b中作用点为血管机器人质心的对偶力旋量，“×”表示对偶四元数的叉乘运算，表示矩阵和对偶四元数的一种运算，对于对偶四元数定义的运算为ε表示对偶单位；定义的运算为为任一对偶四元数；

步骤二所述的重力-浮力补偿器具体设计过程为：

当血管机器人在血液中运动时，定义血管机器人的期望运动速度为在血液中的血管机器人主要受到总驱动力机器人的重力-浮力流体阻力的作用；

总驱动力的表达式为：

其中，表示在三维旋转磁场作用下血管机器人螺旋尾部旋转产生的驱动力，为磁梯度产生的磁拉力，n为血管机器人螺旋尾部的旋转圈数，d为血管机器人磁性球形头部直径，α₁为血管机器人尾部螺旋升角，为血管机器人旋转角速度，V为血管机器人体积，M为磁化强度，为磁场梯度，ζ_⊥为垂直于螺旋尾部的轴向阻力系数，且ζ₁₁为平行于螺旋尾部的轴向阻力系数，且η为血液粘度系数，κ为血管机器人螺旋尾部直径；

流体阻力由血管机器人头部和螺旋尾部受血流冲击所产生的流体阻力组成，表示为：

机器人重力-浮力是重力G和浮力作用在竖直方向上的合力，其表达式为：

其中，ρ为血管机器人的密度，ρ_f为血液的密度，为重力加速度；

抵消重力-浮力对血管机器人的作用效果，需要流体阻力和总驱动力的共同作用，即：

其中

为设计重力-浮力补偿器，建立血管机器人的姿态坐标系p，坐标系的原点O_p位于血管机器人重心，坐标系x_p轴与血管机器人实际指向相重合，z_p轴垂直于血管机器人实际指向为血管机器人为抵消重力-浮力影响的运动速度，

根据式(8)得：

将血管机器人期望运动速度分解为水平分量和竖直分量可知血管机器人期望运动速度的水平分量与的水平分量相等，即有

血管机器人旋转角速度和三维旋转磁场的旋转角速度Ω₁有如下关系：

根据上述分析可得：

其中，为单位矢量；

当时，根据式(9)和(10)可得：

当时，将分解为两个力，即平行于x_p轴的分量与垂直于x_p轴的分量f_⊥p在方向没有分量，又由于磁场梯度方向的不确定性和多变性，假设作用x_pz_p平面上且方向与血管机器人期望运动速度关于x_p轴对称，则类似于的受力分析，可以分解为平行于x_p轴的分量与垂直于x_p轴的分量f_{⊥p_1}在方向没有分量，则有：

其中，和分别表示在z_p轴与x_p轴投影的数量值，和在z_p轴与x_p轴投影的数量值，分别表示z_p轴方向与x_p轴方向的单位向量；将式(10)和(13)代入到式(9)，可得：

由于定义血管机器人期望俯仰角为将速度投影到x_p轴，z_p轴方向，并根据式(14)可得：

通过分析血管机器人姿态坐标系p的力学几何关系可得：

其中，β为血管机器人期望俯仰角向上倾斜的角度；

类似于式(16)中对于的分析，同样可得到以下关系：

根据式(15)，式(16)和式(17)，得到：

根据cos(θ+β)＝cos(θ)cos(β)-sin(θ)sin(β)，则由式(18)可得重力-浮力补偿器输出的血管机器人相比于理想俯仰角向上倾斜的角度β为：

通过式(15)，式(16)和式(17)可得，重力-浮力补偿器输出的三维旋转磁场的旋转角速度Ω₁为：

步骤三所述的基于重力-浮力补偿的滑模控制器为：

记血管机器人期望运动轨迹为[x_d(t),y_d(t),z_d(t)]^T，设机器人初始滚转角速度则血管机器人期望偏航角ψ与期望俯仰角θ分别为：

假设血管机器人的滚转角速度与三维旋转磁场旋转角速度Ω₁一致，则在重力-浮力补偿器作用下血管机器人的偏航角ψ^s、俯仰角θ^s、滚转角速度为：

最后将上述血管机器人期望运动轨迹[x_d(t),y_d(t),z_d(t)]^T与血管机器人经过重力-浮力补偿后的姿态角进行对偶四元数变换，得到对偶四元数框架下的运动描述从而将血管机器人期望坐标系的对偶四元数运动作为控制系统的输入；

血管机器人的控制目标为：通过控制血管机器人本体坐标系O_bX_bY_bZ_b相对于期望坐标系O_dX_dY_dZ_d的姿轨误差使血管机器人的实际运动状态渐进收敛到期望坐标系下的运动状态

基于步骤一中建立的血管机器人姿轨一体化运动学和动力学方程式(3)和式(4)，设计如下滑模面：

其中，为对偶数的积运算，为控制器参数，且k_si＞0，设计滑模控制律为：

其中，为滑模变量，sgn(·)为符号函数，且为控制器参数，且k_εi＞0，为控制器参数，且k_ui＞0，为对偶矢量的一种积运算，为对偶干扰力；

步骤四所述的自适应滑模控制器为：

为便于设计自适应滑模控制器，将式(27)中带有M^b的项用下式表示：

其中，为对偶质量特性惯性矩阵M^b的另一种表达方式；且Π∈R_3×1，通过对偶极子运算的特殊性质ε²＝0可以进一步得到为三维对偶向量；由于对偶质量特性惯性矩阵M^b的特殊性可知Π易得，而关于的计算，可由式(29)-(31)推导得出：对于任意x∈R_3×1,y∈R_3×1，有：

其中，δ(x,y)＝δ₁(x)+δ₂(y)，且δ₁(x)，δ₂(y)的计算方式如下：

由于血管机器人复杂的工作环境及多变的工作内容，在血管机器人建模过程中，其质量与惯量往往具有不确定性与多变性，因此为对偶质量特性惯性矩阵和对偶干扰力的估计值设计参数自适应律如下：

其中，为控制器参数，为控制器参数，k_j＞0,且：

将式(32)和式(33)带入式(27)的滑模控制律，则可得到自适应滑模控制律为：