一种双同步旋转永磁体整体空间位置求解方法及系统与流程

摘要: 本发明涉及微纳机器人磁驱动与运动控制,具体涉及一种双同步旋转永磁体整体空间位置求解方法及系统。背景技术:1、目前,在磁驱动微纳机器人控制领域中,双同步旋转永磁体...
一种双同步旋转永磁体整体空间位置求解方法及系统与流程

本发明涉及微纳机器人磁驱动与运动控制,具体涉及一种双同步旋转永磁体整体空间位置求解方法及系统。

背景技术:

1、目前,在磁驱动微纳机器人控制领域中,双同步旋转永磁体的整体空间位置通常依赖机械结构标定、视觉定位或编码器推算来确定。然而,由于安装误差、机构变形、外部扰动或运动平台误差等因素,双磁体的实际空间位置往往偏离理论位置。同时,在单个时刻下,有限数量的磁传感器观测量与双磁体位置、姿态、旋转相位、磁矩大小及背景磁场等参数之间存在强非线性耦合,导致基于单帧静态磁场数据的反演方法容易出现欠约束、多解或镜像解问题,进而造成磁场模型与实际磁场不一致,影响微纳机器人的控制精度和安全性。因此,急需一种能够充分利用动态磁场信息以消除多解性并提高反演稳定性的双同步旋转永磁体整体空间位置求解方案。

技术实现思路

1、本发明的目的在于,提供一种双同步旋转永磁体整体空间位置求解方法及系统。本发明利用完整旋转周期时间序列增强可观测性,结合刚体约束与多源信息有效排除镜像解,实现高精度、高稳定性的位置反演。

2、本发明的技术方案:一种双同步旋转永磁体整体空间位置求解方法,包括:

3、步骤1、获取双同步旋转永磁体的磁体配置参数以及多个磁传感器在固定坐标系中的空间位置;

4、步骤2、基于呈不共线布置的多个磁传感器,采集双同步旋转永磁体在同步旋转过程中的三轴磁场时间序列;

5、步骤3、基于双同步旋转永磁体的刚体位姿模型和磁场预测模型,确定在给定整体位姿参数下多个磁传感器的检测点处的理论磁场时间序列;

6、步骤4、以所述三轴磁场时间序列与理论磁场时间序列之间的残差构建目标函数,并在预设约束条件下对所述目标函数进行求解,得到所述双同步旋转永磁体的整体空间位置。

7、上述的双同步旋转永磁体整体空间位置求解方法,多个所述磁传感器呈三角形空间布置且位于双同步旋转永磁体之间;

8、所述刚体位姿模型将双同步旋转永磁体中两个永磁体的空间位置表示为双磁体整体位置和整体姿态的函数。

9、前述的双同步旋转永磁体整体空间位置求解方法,所述刚体位姿模型中,所述双磁体整体位置和整体姿态构成双磁体整体位姿,所述双磁体整体位姿表示为,其中,为双磁体整体姿态旋转矩阵,为双磁体整体位置;

10、两个永磁体在固定坐标系中的位置表示为以及;

11、其中,为第一个永磁体在固定坐标系中的位置向量,为第二个永磁体在固定坐标系中的位置向量,为第一个永磁体在双磁体局部坐标系中的中心位置向量,为第二个永磁体在双磁体局部坐标系中的中心位置向量。

12、前述的双同步旋转永磁体整体空间位置求解方法,所述三轴磁场时间序列为至少一个完整旋转周期内的磁场数据;

13、所述目标函数为多个磁传感器在多个采样时刻的实测磁场与理论磁场之间的残差平方和;

14、所述预设约束条件包括位置范围约束、姿态范围约束和量程约束,用于在求解时排除镜像解或非物理解。

15、前述的双同步旋转永磁体整体空间位置求解方法,所述磁场预测模型采用磁偶极子模型、有限尺寸永磁体模型或实测磁场查表模型计算双同步旋转永磁体中单个永磁体产生的磁场;

16、当磁传感器与永磁体之间的距离小于预设距离阈值时,采用所述实测磁场查表模型或有限尺寸永磁体模型,否则采用磁偶极子模型;并将双同步旋转永磁体包含的两个永磁体产生的磁场进行矢量叠加以得到理论总磁场,所述理论总磁场中还叠加有背景磁场项;

17、所述位置范围约束包括双同步旋转永磁体位于多个磁传感器形成的布置区域的预设一侧的约束,以及双同步旋转永磁体与任一所述磁传感器之间的距离处于预设距离范围内的约束。

18、前述的双同步旋转永磁体整体空间位置求解方法,对所述目标函数进行求解的算法包括多初值非线性最小二乘法、全局搜索与局部优化结合算法、扩展卡尔曼滤波算法或粒子滤波算法中的一种或多种;

19、所述目标函数的待估参数集合至少包括所述整体空间位置和整体位姿参数,并包括初始旋转相位、永磁体磁矩大小、背景磁场以及传感器零偏参数中的至少一种。

20、前述的双同步旋转永磁体整体空间位置求解方法,所述磁传感器包括低量程三轴磁场通道和高量程三轴磁场通道;

21、在采集三轴磁场时间序列时,当低量程三轴磁场通道未饱和时采用低量程数据,当低量程三轴磁场通道饱和时高量程数据或进行高低量程融合;

22、在切换量程时,根据低量程三轴磁场通道和高量程三轴磁场通道对应的检测点相对于传感器尖端的位置差异,同步修正磁传感器的检测点的坐标。

23、前述的双同步旋转永磁体整体空间位置求解方法,所述低量程三轴磁场通道的量程为±5mt,所述高量程三轴磁场通道的量程为±130mt;

24、在构建目标函数前,通过无磁体状态测量、静态标定或已知位置磁场标定,获得磁传感器的零偏、比例因子、轴向安装误差和背景磁场,并对采集的所述三轴磁场时间序列进行校正。

25、一种双同步旋转永磁体整体空间位置求解系统,包括:

26、参数获取模块,用于获取双同步旋转永磁体的磁体配置参数以及多个磁传感器在固定坐标系中的空间位置;

27、磁场采集模块,用于基于呈不共线布置的多个磁传感器,采集双同步旋转永磁体在同步旋转过程中的三轴磁场时间序列;

28、磁场预测模块,用于基于双同步旋转永磁体的刚体位姿模型和磁场预测模型,确定在给定整体位姿参数下多个磁传感器的检测点处的理论磁场时间序列;

29、反演求解模块,用于以所述三轴磁场时间序列与理论磁场时间序列之间的残差构建目标函数,并在预设约束条件下对目标函数进行求解,得到双同步旋转永磁体的整体空间位置;

30、结果输出模块,用于输出双同步旋转永磁体的整体空间位置、整体姿态、残差指标和解可信度。

31、前述的双同步旋转永磁体整体空间位置求解系统,所述磁场采集模块与具有唯一识别标识的多个磁传感器通信连接,并根据唯一识别标识区分不同磁传感器的空间位置和三轴磁场时间序列;

32、所述反演求解模块还用于根据多初值求解结果的一致性、目标函数的残差、解是否满足所述预设约束条件以及磁场重构误差,判断求解得到的所述整体空间位置是否可信。

33、与现有技术相比,本发明通过采集双同步旋转永磁体在同步旋转过程中至少一个完整旋转周期的三轴磁场时间序列,并结合呈不共线布置的多个磁传感器的空间分布特征,构建了包含丰富时空动态信息的观测数据集。相较于单帧静态磁场数据,时间序列引入了旋转相位维度的周期性约束,显著增强了系统对双磁体整体位姿参数的可观测性,从根本上缓解了参数间的强非线性耦合导致的欠约束问题。同时,通过建立双同步旋转永磁体的刚体位姿模型,将两个永磁体的空间位置表示为整体位置和整体姿态的函数,有效减少了独立未知量的数量,降低了反演问题的自由度。在此基础上,以实测与理论磁场时间序列的残差构建目标函数,并在位置范围、姿态范围及量程等预设约束条件下进行求解,能够利用空间梯度差异与时间周期动态特征的联合约束,有效排除镜像解和非物理解,从而实现双同步旋转永磁体整体空间位置的高精度、高稳定性反演,为磁驱动微纳机器人的闭环控制和安全校核提供可靠的位置反馈。