一种用于多磁性目标的胶囊机器人定位方法

1.本发明涉及磁性目标定位技术领域，尤其涉及一种用于多磁性目标的胶囊机器人定位方法。


背景技术：

2.磁控胶囊机器人，即在微型胶囊外壳里内置一个永磁体，通过外部的磁场控制永磁体的运动轨迹即可以控制胶囊体的运动，若在胶囊内部放置摄像头、传感器等电子元件便可以完成看诊、治疗等更多的医疗诊疗。而作为医疗领域的新型技术，胶囊机器人近几年发展迅速，在众多的无线技术中，磁控技术能够不受物体遮挡、光线干扰等的限制和约束，相较于x光线、ct检查、核磁共振等检查技术，由磁场控制的胶囊机器人对身体的伤害几乎可以忽略不计。这也是胶囊机器人可以快速应用于医疗诊治手术的原因之一。
3.专利(专利号cn113156525a)公开了一种基于神经网络的水下多磁目标定位方法，利用单目标的磁模型进行融合变成多目标的磁场模型，再使用改进的神经网络和生成的神经网络数据进行训练，这种方法局限于训练好的神经网络数据，需要很复杂的神经网络训练集才有可能保证定位的精度。专利(专利号cn101361660a)公开了一种多磁性目标的定位方法及定位系统，利用非线性优化算法对物体进行定位，根据目标的绝对坐标系位置进行相对定位，这种方法对优化算法的要求高，需要有一个较好的初值，否则只能进行相对定位。
4.采用上述方式，当初值不合适会陷入局部最优的不足，可能导致整体定位效果较差。


技术实现要素：

5.本发明的目的在于提供一种用于多磁性目标的胶囊机器人定位方法，旨在解决现有的定位方法当初值不合适会陷入局部最优的不足，可能导致整体定位效果较差的问题。
6.为实现上述目的，本发明提供了一种用于多磁性目标的胶囊机器人定位方法，包括以下步骤：
7.建立磁传感器坐标系、世界坐标系、永磁体坐标系和数学模型；
8.将多磁体目标的姿态数据和磁场数据代入神经网络算法中计算，得到多磁体目标的初始位姿；
9.使用改进的优化算法对所述磁传感器理论值和实测值进行迭代优化，完成定位。
10.其中，所述将多磁体目标的姿态数据和磁场数据代入神经网络算法中计算，得到多磁体目标的初始位姿，包括：
11.对所述世界坐标系下的已知多磁体目标姿态数据通过变换，得到用于改进初值算法的姿态数据；
12.根据所述世界坐标系下的所述磁传感器坐标得到已知多磁体目标姿态数据对应的磁场数据；
13.将所述的已知多磁体目标姿态数据和与其对应的磁场数据带入所述算法模型进行计算，得到多磁体目标第一姿态值。
14.其中，所述根据所述世界坐标系下的所述磁传感器坐标得到已知多磁体目标姿态数据对应的磁场数据，包括：
15.对所述世界坐标系下的磁传感器坐标通过变换矩阵转换，得到永磁体坐标系下磁传感器坐标；
16.基于所述永磁体坐标系下磁传感器坐标和永磁体积分模型进行计算，得到第一磁场值；
17.将所述第一磁场值通过旋转矩阵变换到所述世界坐标系下，得到已知多磁体目标姿态数据对应的磁场数据。
18.其中，所述改进的优化算法对所述磁传感器理论值和实测值进行迭代优化，完成定位，包括:
19.根据所述磁传感器的理论值和实测值建立误差函数；
20.对所述误差函数进行迭代优化，完成定位。
21.其中，所述误差函数是磁传感器阵列测得的值和利用模型和算法计算出来的磁场数据作差值。
22.本发明的一种用于多磁性目标的胶囊机器人定位方法，建立磁传感器坐标系、世界坐标系、永磁体坐标系和数学模型；将多磁体目标的姿态数据和磁场数据代入所述数学模型和算法中计算，得到多磁体目标的第一姿态值；使用改进式的优化算法对所述磁传感器理论值和实测值建立的优化函数进行迭代优化，将所述多磁体目标的第一姿态值作为算法的迭代初值，完成定位，基于常用的levenberg-marquarelt优化算法进行改进，实现一种两个及以上的多磁性目标的胶囊机器人精确定位方法。首先建立磁场坐标系和多磁性目标的数学模型；其次使用神经网络算法生成永磁体初值，利用数学模型计算对应的磁传感器位置上的磁场值，建立误差函数；最后，使用算法迭代公式更新误差函数使得其收敛，在不断优化误差函数的过程中达到精确定位的目的，使得在保证速度的情况下，精度更高，从而解决了现有的定位方法当初值不合适会陷入局部最优的不足，可能导致整体定位效果较差的问题。
附图说明
23.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
24.图1是本发明方法的流程图；
25.图2为构建的坐标系图例；
26.图3为毕奥萨伐尔定律的积分模型示意图；
27.图4是本发明提供的一种用于多磁性目标的胶囊机器人定位方法的流程图。
具体实施方式
28.下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
29.请参阅图1至图4，本发明提供一种用于多磁性目标的胶囊机器人定位方法，包括以下步骤：
30.s1建立磁传感器坐标系、世界坐标系、永磁体坐标系和数学模型；
31.具体的，在磁传感器阵列中，一共有9个磁传感器，以50mm的间隔排列，每个传感器都能测出x、y、z三个方向的磁场数据，而世界坐标系的方向与磁传感器测出的方向相同，原点与第一个磁传感器坐标系重合。永磁体坐标系的原点是磁体的质心，轴向为z轴建立永磁体坐标系。在世界坐标系下，磁传感器阵列的位置是已知的，永磁体的位姿是未知的需要求解的。在永磁体坐标系中，建立的数学模型是基于毕奥萨伐尔定律的，其原理如下：如图3所示是一个轴向均匀磁化的圆柱形永磁体，以永磁体的质心为原点建立坐标系，圆柱形永磁体的半径为r，高度为h，磁矩的方向与z轴方向相同。在圆柱体中，取一个厚度为dz
′
的圆形薄磁铁，表面电流为i，令空间中的一点p的坐标为p(x,y,z)，对于厚度为dz
′
的圆形薄磁铁，根据毕奥撒伐尔定律，在p点处产生的磁场强度计算公式为：
[0032][0033]
其中，μo为真空磁导率，其值为4π
×
10-7
t
·
m/a，是圆形薄磁铁上的一段微元，表示为从圆上的一点指向p的向量。在均匀磁化的圆柱形永磁体中，等效体电流密度永磁体表面的等效电流密度为为圆柱体侧边圆环的切线方向，m为永磁体的磁矩，在厚度为dz
′
的圆形薄磁铁上，圆环的等效电流di＝m
t
dz
′
，m
t
为永磁体的磁矩大小。所以圆柱形磁铁在p点处产生的磁场可以写为：
[0034][0035]
其中θ表示为该圆形薄线圈的圆心角。
[0036]
s2将多磁体目标的姿态数据和磁场数据代入所述神经网络算法中计算，得到多磁体目标的姿态初值；
[0037]
s21对所述世界坐标系下的已知多磁体目标姿态数据通过变换，得到用于改进初值算法的姿态数据；
[0038]
s22根据所述世界坐标系下的所述磁传感器坐标得到已知多磁体目标姿态数据对应的磁场数据；
[0039]
具体的，用软件得到所有可能的多目标磁体的姿态数据，即多目标磁体的位置和姿态已知，那么根据已知的世界坐标系下磁传感器坐标，以第i个传感器为例，假设第i个传感器在世界坐标系中的位置是pi(xi,yi,zi)，则在永磁体坐标系下磁传感器的位置坐标pi'(xi',yi',zi')可以由变换矩阵得到；其次，在永磁体坐标系下，根据上一步得到的磁传感器的位置坐标pi'(xi',yi',zi')和永磁体积分模型便可以计算出永磁体在磁传感器位置处产生的磁场最后，将磁场值通过旋转矩阵变换到世界坐标系下，便可以得到世界坐标系下永磁体位置在磁传感器位置
[0040]
处产生的理论磁场
[0041]
s221对所述世界坐标系下的磁传感器坐标通过变换矩阵转换，得到永磁体坐标系下磁传感器坐标；
[0042]
具体的，根据世界坐标系下磁传感器坐标已知可以得到，以第i个传感器为例，假设第i个传感器在世界坐标系中的位置是pi(xi,yi,zi)，则在永磁体坐标系下磁传感器的位置坐标p'i(x'i,y'i,z'i)可以由变换矩阵得到。
[0043]
s222基于所述永磁体坐标系下磁传感器坐标和永磁体积分模型进行计算，得到第一磁场值；
[0044]
具体的，在永磁体坐标系下，根据建立的永磁体积分模型和永磁体坐标系下磁传感器的位置坐标pi'(xi',yi',zi')可以计算出在永磁体坐标系下永磁体在磁传感器位置处产生的磁场。
[0045]
s223将所述第一磁场值通过旋转矩阵变换到所述世界坐标系下，得到磁场数据。
[0046]
具体的，再将磁场值通过旋转矩阵变换到世界坐标系下，便可以得到在世界坐标系下永磁体位置在磁传感器位置处产生的磁场值。
[0047]
s23将所述的已知多磁体目标姿态数据和与其对应的磁场数据带入所述算法模型进行计算，得到多磁体目标第一姿态值。
[0048]
具体的，将所述的多磁体目标第一姿态值用于l-m优化算法的初值，将初值代入数学模型中计算相对应的磁传感器理论值。
[0049]
s3使用改进的优化算法对所述磁传感器理论值和实测值进行迭代优化，完成定位。
[0050]
s31根据所述磁传感器的理论值和实测值建立误差函数；
[0051]
所述误差函数是磁传感器阵列测得的值和利用模型计算出来的磁场数据作差值。
[0052]
具体的，磁传感器阵列实时采集的空间中多磁体目标产生的磁场后，通过单片机将数据传输到微型计算机中进行滤波处理，处理结果记为同时对步骤二中所得到的理论磁场进行处理即每一个磁传感器位置处多目标永磁体产生的磁感应强度的叠加，记为则可以定义误差函数为：
[0053][0054]
其中m表示为多目标磁体的个数。n表示为磁传感器阵列中磁传感器的个数。
[0055]
s32对所述误差函数进行迭代优化，完成定位。
[0056]
具体的，使用l-m算法将该误差函数进行优化，不断更新永磁体的位姿数据，使得误差函数无限收敛，最后得到最终多磁体目标的姿态值。
[0057]
有益效果：
[0058]
l-m优化算法因其速度高通常被用于单目标的定位优化，但其局限于初值的选择而精确度不高，故其用在多目标的定位上弊端就更加显而易见。本发明所提出的多磁性目标的定位方法速度快、精度高，加入神经网络算法确定l-m优化算法的初值，使得在保证原来的迭代优化速度的情况下，优化精度更高，解决了l-m算法计算速度快但是若初值不合适会陷入局部最优的不足；在此基础上，还对用于单目标定位的优化步骤进行了改进，使其在多目标定位领域也同样适用。相较于其他使用l-m算法进行定位的方法来说，本发明对整体的算法进行了改进，使得l-m优化算法不仅能适用于多磁体目标定位，还能保持高效的迭代速度，完成精确的定位效果。
[0059]
以上所揭露的仅为本发明一种用于多磁性目标的胶囊机器人定位方法较佳实施例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。

技术特征：
1.一种用于多磁性目标的胶囊机器人定位方法，其特征在于，包括以下步骤：建立磁传感器坐标系、世界坐标系、永磁体坐标系和数学模型；将多磁体目标的姿态数据和磁场数据代入神经网络算法中计算，得到多磁体目标的初始位姿；使用改进的优化算法对所述磁传感器理论值和实测值进行迭代优化，完成定位。2.如权利要求1所述的一种用于多磁性目标的胶囊机器人定位方法，其特征在于，所述将多磁体目标的姿态数据和磁场数据代入神经网络算法中计算，得到多磁体目标的初始位姿，包括：对所述世界坐标系下的已知多磁体目标姿态数据通过变换，得到用于改进初值算法的姿态数据；根据所述世界坐标系下的所述磁传感器坐标得到已知多磁体目标姿态数据对应的磁场数据；将所述的已知多磁体目标姿态数据和与其对应的磁场数据带入所述算法模型进行计算，得到多磁体目标第一姿态值。3.如权利要求2所述的一种用于多磁性目标的胶囊机器人定位方法，其特征在于，所述根据所述世界坐标系下的所述磁传感器坐标得到已知多磁体目标姿态数据对应的磁场数据，包括：对所述世界坐标系下的磁传感器坐标通过变换矩阵转换，得到永磁体坐标系下磁传感器坐标；基于所述永磁体坐标系下磁传感器坐标和永磁体积分模型进行计算，得到第一磁场值；将所述第一磁场值通过旋转矩阵变换到所述世界坐标系下，得到已知多磁体目标姿态数据对应的磁场数据。4.如权利要求3所述的一种用于多磁性目标的胶囊机器人定位方法，其特征在于，所述使用改进的优化算法对所述磁传感器理论值和实测值进行迭代优化，完成定位，包括：根据所述磁传感器的理论值和实测值建立误差函数；对所述误差函数进行迭代优化，完成定位。5.如权利要求4所述的一种用于多磁性目标的胶囊机器人定位方法，其特征在于，所述误差函数是磁传感器阵列测得的值和利用模型和算法计算出来的磁场数据作差值。

技术总结
本发明涉及磁性目标定位技术领域，具体涉及一种用于多磁性目标的胶囊机器人定位方法，本方法基于常用的Levenberg-Marquarelt优化算法进行改进，实现一种两个及以上的多磁性目标的胶囊机器人精确定位方法。首先建立磁场坐标系和多磁性目标的数学模型；其次使用神经网络算法生成永磁体初值，利用数学模型计算对应的磁传感器位置上的磁场值，建立误差函数；最后，使用算法迭代公式更新误差函数使得其收敛，在不断优化误差函数的过程中达到精确定位的目的，从而解决了现有的定位方法当初值不合适会陷入局部最优的不足，可能导致整体定位效果较差的问题。果较差的问题。果较差的问题。


技术研发人员：张琦 朱莉
受保护的技术使用者：桂林电子科技大学
技术研发日：2023.04.19
技术公布日：2023/8/5