本发明涉及胶囊机器人姿态测定技术，具体是一种基于传感器和一维线圈的胶囊机器人姿态测定系统及方法。
背景技术：
胶囊机器人是一种可经口腔或肛门进入人体肠道的微型机器人，其可在肠道中自主运动，完成对肠道内腔的无创诊查。在诊查过程中，胶囊机器人的姿态会随着所处肠道环境的变化而发生随机变化，导致其无法对病灶进行定位。为此，需要对胶囊机器人的姿态进行测定，以实现对病灶的定位。在现有技术条件下，胶囊机器人的姿态测定主要采用九轴姿态传感器(由三轴加速度计、三轴陀螺仪、三轴磁强计组成)进行。然而在实际应用中，现有胶囊机器人姿态测定技术由于自身原理所限，存在如下问题：其一，胶囊机器人采用无线电能传输系统(由无线电能发射线圈和无线电能接收线圈组成)进行供电。在无线电能传输系统中，无线电能的传输媒介为交变磁场，交变磁场会对九轴姿态传感器中的三轴磁强计造成严重的磁干扰，由此导致三轴磁强计的测量结果不准确，从而导致姿态测定结果不准确。其二，现有胶囊机器人姿态测定技术在对胶囊机器人的姿态角进行解算时，存在运算量大的问题，因此其不利于实现姿态的实时测定。基于此，有必要发明一种基于传感器和一维线圈的胶囊机器人姿态测定系统及方法，以解决现有胶囊机器人姿态测定技术姿态测定结果不准确、不利于实现姿态的实时测定的问题。
技术实现要素：
本发明为了解决现有胶囊机器人姿态测定技术姿态测定结果不准确、不利于实现姿态的实时测定的问题，提供了一种基于传感器和一维线圈的胶囊机器人姿态测定系统及方法。
本发明是采用如下技术方案实现的：
基于传感器和一维线圈的胶囊机器人姿态测定系统，包括置于体外的三维无线电能发射线圈、安装于胶囊机器人上的一维无线电能接收线圈、安装于胶囊机器人上的电压调理模块、安装于胶囊机器人上的六轴姿态传感器、安装于胶囊机器人上的主控单片机、安装于胶囊机器人上的无线信号发射模块、置于体外的无线信号接收模块、置于体外的上位机；
一维无线电能接收线圈的中心轴线与胶囊机器人的中心轴线重合；电压调理模块的输入端与一维无线电能接收线圈的一端连接；电压调理模块的输出端与主控单片机的输入端连接；
所述六轴姿态传感器包括三轴加速度计、三轴陀螺仪；
三轴加速度计的x轴、三轴陀螺仪的x轴均与胶囊机器人的中心轴线平行；三轴加速度计的输出端、三轴陀螺仪的输出端均与主控单片机的输入端连接；
主控单片机的输出端与无线信号发射模块的输入端连接；无线信号发射模块的输出端与无线信号接收模块的输入端无线连接；无线信号接收模块的输出端与上位机的输入端连接。
所述六轴姿态传感器为mpu6050型六轴姿态传感器。
基于传感器和一维线圈的胶囊机器人姿态测定方法(该方法是基于本发明所述的基于传感器和一维线圈的胶囊机器人姿态测定系统实现的)，该方法是采用如下步骤实现的：
步骤一：三维无线电能发射线圈激发出交变磁场；一维无线电能接收线圈在交变磁场的作用下产生感应电压；感应电压先经电压调理模块进行调理，再经主控单片机进行ad转换，然后依次经无线信号发射模块、无线信号接收模块发送至上位机；与此同时，三轴加速度计实时输出三轴加速度信息，三轴陀螺仪实时输出三轴角速度信息，三轴加速度信息、三轴角速度信息均依次经主控单片机、无线信号发射模块、无线信号接收模块发送至上位机；
步骤二：上位机根据三轴加速度信息、三轴角速度信息、感应电压实时解算出胶囊机器人的横滚角α、俯仰角β、航向角γ，并将解算结果进行实时显示和存储，由此对胶囊机器人的姿态进行实时测定；具体解算步骤如下：
1)根据三轴加速度信息解算出横滚角αa、俯仰角βa；具体解算公式如下：
αa＝tan-1(ax/az)(1)；
βa＝tan-1(ay/az)(2)；
式(1)～式(2)中：ax、ay、az分别表示x轴加速度信息、y轴加速度信息、z轴加速度信息；
2)根据三轴角速度信息解算出横滚角αg、俯仰角βg、航向角γg；具体解算公式如下：
αg＝∫gxdt α(0)(3)；
βg＝∫gydt β(0)(4)；
γg＝∫gzdt γ(0)(5)；
式(3)～式(5)中：gx、gy、gz分别表示x轴角速度信息、y轴角速度信息、z轴角速度信息；α(0)、β(0)、γ(0)分别表示当前时刻横滚角、俯仰角、航向角的积分初值；
3)利用互补滤波融合算法将横滚角αa和横滚角αg进行数据融合，由此得到横滚角α；利用互补滤波融合算法将俯仰角βa和俯仰角βg进行数据融合，由此得到俯仰角β；
4)根据感应电压的幅值ε、交变磁场的参数、一维无线电能接收线圈的参数解算出磁偏角θ；具体解算公式如下：
式(6)中：
5)根据磁偏角θ、横滚角α、俯仰角β、交变磁场矢量的三轴分量解算出航向角γε；具体解算公式如下：
式(7)～式(8)中：bx、by、bz分别表示交变磁场矢量的x轴分量、y轴分量、z轴分量；bx、by、bz均为已知量；
6)利用互补滤波融合算法将航向角γg和航向角γε进行数据融合，由此得到航向角γ；
步骤三：将实时解算出的横滚角α、俯仰角β、航向角γ分别作为下一时刻横滚角、俯仰角、航向角的积分初值，并重复执行步骤二至步骤三，由此持续对胶囊机器人的姿态进行实时测定。
所述步骤4)～步骤5)中，磁偏角θ为交变磁场矢量与一维无线电能接收线圈的中心轴线的夹角。
所述步骤5)中，由式(7)可解算出航向角γε的两个解，由式(8)同样可解算出航向角γε的两个解，式(7)和式(8)的公共解即为航向角γε的真值。
与现有胶囊机器人姿态测定技术相比，本发明所述的基于传感器和一维线圈的胶囊机器人姿态测定系统及方法具备了如下优点：其一，本发明不再采用九轴姿态传感器，而是利用六轴姿态传感器的输出信息(三轴加速度信息和三轴角速度信息)和一维无线电能接收线圈的感应电压实现了胶囊机器人的姿态测定，因此其不会被无线电能传输系统的交变磁场干扰，从而使得姿态测定结果更准确。其二，本发明仅仅利用简单的三角函数运算，即可实现胶囊机器人的姿态角解算，因此其运算量更小，从而有利于实现姿态的实时测定。
本发明有效解决了现有胶囊机器人姿态测定技术姿态测定结果不准确、不利于实现姿态的实时测定的问题，适用于胶囊机器人的姿态测定。
附图说明
图1是本发明中基于传感器和一维线圈的胶囊机器人姿态测定系统的结构示意图。
图2是本发明中一维无线电能接收线圈和六轴姿态传感器在胶囊机器人上的安装示意图。
图3是本发明中基于传感器和一维线圈的胶囊机器人姿态测定方法的步骤二至步骤三的流程示意图。
具体实施方式
基于传感器和一维线圈的胶囊机器人姿态测定系统，包括置于体外的三维无线电能发射线圈、安装于胶囊机器人上的一维无线电能接收线圈、安装于胶囊机器人上的电压调理模块、安装于胶囊机器人上的六轴姿态传感器、安装于胶囊机器人上的主控单片机、安装于胶囊机器人上的无线信号发射模块、置于体外的无线信号接收模块、置于体外的上位机；
一维无线电能接收线圈的中心轴线与胶囊机器人的中心轴线重合；电压调理模块的输入端与一维无线电能接收线圈的一端连接；电压调理模块的输出端与主控单片机的输入端连接；
所述六轴姿态传感器包括三轴加速度计、三轴陀螺仪；
三轴加速度计的x轴、三轴陀螺仪的x轴均与胶囊机器人的中心轴线平行；三轴加速度计的输出端、三轴陀螺仪的输出端均与主控单片机的输入端连接；
主控单片机的输出端与无线信号发射模块的输入端连接；无线信号发射模块的输出端与无线信号接收模块的输入端无线连接；无线信号接收模块的输出端与上位机的输入端连接。
所述六轴姿态传感器为mpu6050型六轴姿态传感器。
基于传感器和一维线圈的胶囊机器人姿态测定方法(该方法是基于本发明所述的基于传感器和一维线圈的胶囊机器人姿态测定系统实现的)，该方法是采用如下步骤实现的：
步骤一：三维无线电能发射线圈激发出交变磁场；一维无线电能接收线圈在交变磁场的作用下产生感应电压；感应电压先经电压调理模块进行调理，再经主控单片机进行ad转换，然后依次经无线信号发射模块、无线信号接收模块发送至上位机；与此同时，三轴加速度计实时输出三轴加速度信息，三轴陀螺仪实时输出三轴角速度信息，三轴加速度信息、三轴角速度信息均依次经主控单片机、无线信号发射模块、无线信号接收模块发送至上位机；
步骤二：上位机根据三轴加速度信息、三轴角速度信息、感应电压实时解算出胶囊机器人的横滚角α、俯仰角β、航向角γ，并将解算结果进行实时显示和存储，由此对胶囊机器人的姿态进行实时测定；具体解算步骤如下：
1)根据三轴加速度信息解算出横滚角αa、俯仰角βa；具体解算公式如下：
αa＝tan-1(ax/az)(1)；
βa＝tan-1(ay/az)(2)；
式(1)～式(2)中：ax、ay、az分别表示x轴加速度信息、y轴加速度信息、z轴加速度信息；
2)根据三轴角速度信息解算出横滚角αg、俯仰角βg、航向角γg；具体解算公式如下：
αg＝∫gxdt α(0)(3)；
βg＝∫gydt β(0)(4)；
γg＝∫gzdt γ(0)(5)；
式(3)～式(5)中：gx、gy、gz分别表示x轴角速度信息、y轴角速度信息、z轴角速度信息；α(0)、β(0)、γ(0)分别表示当前时刻横滚角、俯仰角、航向角的积分初值；
3)利用互补滤波融合算法将横滚角αa和横滚角αg进行数据融合，由此得到横滚角α；利用互补滤波融合算法将俯仰角βa和俯仰角βg进行数据融合，由此得到俯仰角β；
4)根据感应电压的幅值ε、交变磁场的参数、一维无线电能接收线圈的参数解算出磁偏角θ；具体解算公式如下：
式(6)中：
5)根据磁偏角θ、横滚角α、俯仰角β、交变磁场矢量的三轴分量解算出航向角γε；具体解算公式如下：
式(7)～式(8)中：bx、by、bz分别表示交变磁场矢量的x轴分量、y轴分量、z轴分量；bx、by、bz均为已知量；
6)利用互补滤波融合算法将航向角γg和航向角γε进行数据融合，由此得到航向角γ；
步骤三：将实时解算出的横滚角α、俯仰角β、航向角γ分别作为下一时刻横滚角、俯仰角、航向角的积分初值，并重复执行步骤二至步骤三，由此持续对胶囊机器人的姿态进行实时测定。
所述步骤4)～步骤5)中，磁偏角θ为交变磁场矢量与一维无线电能接收线圈的中心轴线的夹角。
所述步骤5)中，由式(7)可解算出航向角γε的两个解，由式(8)同样可解算出航向角γε的两个解，式(7)和式(8)的公共解即为航向角γε的真值。
虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这些仅是举例说明，本发明的保护范围是由所附权利要求书限定的。本领域的技术人员在不背离本发明的原理和实质的前提下，可以对这些实施方式作出多种变更或修改，但这些变更和修改均落入本发明的保护范围。
1.一种基于传感器和一维线圈的胶囊机器人姿态测定系统，其特征在于：包括置于体外的三维无线电能发射线圈、安装于胶囊机器人上的一维无线电能接收线圈、安装于胶囊机器人上的电压调理模块、安装于胶囊机器人上的六轴姿态传感器、安装于胶囊机器人上的主控单片机、安装于胶囊机器人上的无线信号发射模块、置于体外的无线信号接收模块、置于体外的上位机；
一维无线电能接收线圈的中心轴线与胶囊机器人的中心轴线重合；电压调理模块的输入端与一维无线电能接收线圈的一端连接；电压调理模块的输出端与主控单片机的输入端连接；
所述六轴姿态传感器包括三轴加速度计、三轴陀螺仪；
三轴加速度计的x轴、三轴陀螺仪的x轴均与胶囊机器人的中心轴线平行；三轴加速度计的输出端、三轴陀螺仪的输出端均与主控单片机的输入端连接；
主控单片机的输出端与无线信号发射模块的输入端连接；无线信号发射模块的输出端与无线信号接收模块的输入端无线连接；无线信号接收模块的输出端与上位机的输入端连接。
2.根据权利要求1所述的基于传感器和一维线圈的胶囊机器人姿态测定系统，其特征在于：所述六轴姿态传感器为mpu6050型六轴姿态传感器。
3.一种基于传感器和一维线圈的胶囊机器人姿态测定方法，该方法是基于如权利要求1所述的基于传感器和一维线圈的胶囊机器人姿态测定系统实现的，其特征在于：该方法是采用如下步骤实现的：
步骤一：三维无线电能发射线圈激发出交变磁场；一维无线电能接收线圈在交变磁场的作用下产生感应电压；感应电压先经电压调理模块进行调理，再经主控单片机进行ad转换，然后依次经无线信号发射模块、无线信号接收模块发送至上位机；与此同时，三轴加速度计实时输出三轴加速度信息，三轴陀螺仪实时输出三轴角速度信息，三轴加速度信息、三轴角速度信息均依次经主控单片机、无线信号发射模块、无线信号接收模块发送至上位机；
步骤二：上位机根据三轴加速度信息、三轴角速度信息、感应电压实时解算出胶囊机器人的横滚角α、俯仰角β、航向角γ，并将解算结果进行实时显示和存储，由此对胶囊机器人的姿态进行实时测定；具体解算步骤如下：
1)根据三轴加速度信息解算出横滚角αa、俯仰角βa；具体解算公式如下：
αa＝tan-1(ax/az)(1)；
βa＝tan-1(ay/az)(2)；
式(1)～式(2)中：ax、ay、az分别表示x轴加速度信息、y轴加速度信息、z轴加速度信息；
2)根据三轴角速度信息解算出横滚角αg、俯仰角βg、航向角γg；具体解算公式如下：
αg＝∫gxdt α(0)(3)；
βg＝∫gydt β(0)(4)；
γg＝∫gzdt γ(0)(5)；
式(3)～式(5)中：gx、gy、gz分别表示x轴角速度信息、y轴角速度信息、z轴角速度信息；α(0)、β(0)、γ(0)分别表示当前时刻横滚角、俯仰角、航向角的积分初值；
3)利用互补滤波融合算法将横滚角αa和横滚角αg进行数据融合，由此得到横滚角α；利用互补滤波融合算法将俯仰角βa和俯仰角βg进行数据融合，由此得到俯仰角β；
4)根据感应电压的幅值ε、交变磁场的参数、一维无线电能接收线圈的参数解算出磁偏角θ；具体解算公式如下：
式(6)中：
5)根据磁偏角θ、横滚角α、俯仰角β、交变磁场矢量的三轴分量解算出航向角γε；具体解算公式如下：
式(7)～式(8)中：bx、by、bz分别表示交变磁场矢量的x轴分量、y轴分量、z轴分量；bx、by、bz均为已知量；
6)利用互补滤波融合算法将航向角γg和航向角γε进行数据融合，由此得到航向角γ；
步骤三：将实时解算出的横滚角α、俯仰角β、航向角γ分别作为下一时刻横滚角、俯仰角、航向角的积分初值，并重复执行步骤二至步骤三，由此持续对胶囊机器人的姿态进行实时测定。
4.根据权利要求3所述的基于传感器和一维线圈的胶囊机器人姿态测定方法，其特征在于：所述步骤4)～步骤5)中，磁偏角θ为交变磁场矢量与一维无线电能接收线圈的中心轴线的夹角。
5.根据权利要求3所述的基于传感器和一维线圈的胶囊机器人姿态测定方法，其特征在于：所述步骤5)中，由式(7)可解算出航向角γε的两个解，由式(8)同样可解算出航向角γε的两个解，式(7)和式(8)的公共解即为航向角γε的真值。
技术总结