[发明专利]磁引导式胶囊内窥镜系统及轨迹规划方法

权利要求书

1.一种磁引导式胶囊内窥镜系统，其特征在于：包括病床(1)、磁铁控制模块(2)、磁引导式胶囊机器人(3)、外部引导磁铁(4)、相机模块(5)和人机交互系统(6)，所述的外部引导磁铁(4)安装在磁铁控制模块(2)上与置于人体内的磁引导式胶囊机器人(3)内装磁铁产生交互磁场，所述的相机模块(5)由多台位于病床(1)支架上的相机组成，相机模块通过多目视觉建立工作环境的三维模型；相机模块(5)和磁铁控制模块(2)均与人机交互系统(6)连接，在工作环境的三维模型中提取病人人体模型，将所提取病人的人体模型离散化后存入人机交互系统，确定磁引导式胶囊机器人目前所在位置与姿态，确定磁引导式胶囊机器人下一步的前进方向及前进距离，即确定磁引导式胶囊机器人的前进向量，并存入人机交互系统；在人体离散化模型的部分区域上选取外部引导磁铁方向点,用于确定胶囊机器人的旋转角度；人机交互系统控制磁铁控制模块带动外部引导磁铁从当前位置沿直线运动到达外部引导磁铁的过渡位置及目标姿态，人体内的磁引导式胶囊机器人被带动到达磁引导式胶囊机器人的目标姿态；人机交互系统控制磁铁控制模块带动外部引导磁铁沿所述前进向量平移至目标位置Ⅱ，胶囊机器人被引导至目标位置Ⅰ；

外部引导磁铁(4)的Z轴与磁引导式胶囊机器人(3)的Z轴重合，为磁引导式胶囊机器人(3)的当前位置Ⅰ(9)到所选取的外部引导磁铁方向点(12)的连线方向；外部引导磁铁(4)的Y轴与磁引导式胶囊机器人(3)的Y轴平行，为胶囊机器人前进向量(8)所指向的方向；外部引导磁铁(4)的X轴与磁引导式胶囊机器人(3)的X轴平行；外部引导磁铁(4)的过渡位置(13)为：以胶囊机器人的当前位置Ⅰ(9)到所选取的外部引导磁铁方向点(12)为正方向，距离磁引导式胶囊机器人(3)的当前位置Ⅰ(9)距离为d的点，磁引导式胶囊机器人(3)的目标姿态(14)与外部引导磁铁(4)的目标姿态(14)一致，所述的d取值为max{L+R,D},其中L为外部引导磁铁方向点(12)到磁引导式胶囊机器人(3)当前位置Ⅰ(9)之间的距离，R为外部引导磁铁(4)的半径，D为磁引导式胶囊机器人(3)与外部引导磁铁(4)之间的预估距离，预先存储至人机交互系统(6)；

过磁引导式胶囊机器人(3)的当前位置Ⅰ(9)做与磁引导式胶囊机器人(3)的前进向量(8)垂直的平面A(17)，过引导式胶囊机器人的前进向量(8)做与世界坐标系x轴垂直的平面B(19)，做与平面A距离为小量δ的两个两个平面A₁(16)、平面A₂(18)，做与平面B距离为v的两个平面B₁(20)、平面B₂(21)，平面A₁(16)、平面A₂(18)、平面B₁(20)、平面B₂(21)围成的离散化模型的部分区域；

相机模块(5)刷新周边环境模型，并对所述外部引导磁铁(4)的运动过程进行仿真同时判断是否有碰撞发生，若有碰撞发生，磁铁控制模块(2)停止运动，取与磁引导式胶囊机器人(3)的当前位置点Ⅰ(9)距离次短的点为外部引导磁铁方向点(12)，并按上述方法计算外部引导磁铁(4)的运动轨迹。