[发明专利]一种胶囊内镜运动控制方法及系统

权利要求书

1.一种胶囊内镜运动控制系统，其特征在于，包括：

电磁线圈：用于产生磁场，所述电磁线圈包括用于产生均匀场强梯度磁场的梯度线圈和用于产生均匀磁场强度磁场的匀场线圈，所述磁场包括所述均匀场强梯度磁场和所述均匀磁场强度磁场叠加的组合磁场；

胶囊内镜：用于在所述磁场的作用下在受检者消化道内运动；所述胶囊内镜内置有传感器；

所述传感器：用于检测所述胶囊内镜的实际运动状态，所述传感器包括三轴加速度计和三轴角速度计；

驱动电路：用于向所述电磁线圈提供供电电压；

CPU：用于接收所述胶囊内镜的实际运动状态，所述实际运动状态包括所述三轴加速度计检测到的所述胶囊内镜的实际加速度和所述三轴角速度计检测到的所述胶囊内镜的实际角速度，根据所述实际运动状态和输入的期望运动状态，利用控制模型计算所述电磁线圈的控制电压；根据所述控制电压调节所述驱动电路为所述电磁线圈提供的供电电压，以便改变所述胶囊内镜的受力状况，直至所述胶囊内镜达到所述期望运动状态；所述期望运动状态包括期望位置和期望姿态角；所述控制模型包括位置控制模型和姿态控制模型；

所述CPU具体用于：

将所述实际加速度经二次积分计算得到所述胶囊内镜当前时刻的实际位置；根据所述实际位置进行受力分析计算得到所述梯度线圈的平衡电流；根据所述实际位置和所述期望位置计算得到位置误差；将所述位置误差代入所述位置控制模型中计算得到所述梯度线圈的调节电流；根据所述平衡电流和所述调节电流计算得到所述梯度线圈的控制电流；根据所述梯度线圈的控制电流和驱动所述梯度线圈的电阻阻值，计算得到当前时刻所述梯度线圈的控制电压；

以及，根据所述实际加速度计算得到所述胶囊内镜的初始姿态角；根据所述初始姿态角和所述实际角速度计算得到所述胶囊内镜当前时刻的实际姿态角；根据所述实际姿态角和所述期望姿态角计算得到姿态角误差；将所述姿态角误差代入所述姿态控制模型中计算得到所述匀场线圈的控制电流；根据所述匀场线圈的控制电流和驱动所述匀场线圈的电阻阻值，计算得到当前时刻所述匀场线圈的控制电压。

2.根据权利要求1所述胶囊内镜运动控制系统，其特征在于，所述CPU具体用于：

根据所述初始姿态角确定所述胶囊内镜初始时刻姿态的四元数；根据所述初始时刻姿态的四元数和所述实际角速度，计算得到所述胶囊内镜当前时刻姿态的四元数；根据所述当前时刻姿态的四元数计算得到所述胶囊内镜当前时刻的实际姿态角。

3.根据权利要求1所述胶囊内镜运动控制系统，其特征在于，所述CPU具体用于：

根据所述实际运动状态和输入的期望运动状态，在所述控制模型中采用积分分离且带死区的PID控制算法计算所述电磁线圈的控制电压。