一种机械臂搭载永磁体的磁微纳机器人协同运动控制方法与流程

摘要: 本发明涉及磁驱动微纳机器人及机器人控制,具体涉及一种机械臂搭载永磁体的磁微纳机器人协同运动控制方法。背景技术:1、目前,在磁微纳机器人驱动控制领域中,通常采用固...
一种机械臂搭载永磁体的磁微纳机器人协同运动控制方法与流程

本发明涉及磁驱动微纳机器人及机器人控制,具体涉及一种机械臂搭载永磁体的磁微纳机器人协同运动控制方法。

背景技术:

1、目前,在磁微纳机器人驱动控制领域中,通常采用固定式双同步旋转永磁体产生旋转磁场以驱动螺旋型磁微纳机器人进行靶向递送或微创作业。然而,固定式磁驱动装置的有效工作空间受限于永磁体的几何布局与磁场分布特性,当磁微纳机器人沿较长路径运动时,容易偏离优选驱动区域,导致局部磁场强度、旋转轴方向及磁场梯度发生显著变化,进而降低运动控制的精度与稳定性。同时,现有控制策略往往将机械臂位姿调节与磁体转速调节视为独立或简单串联的控制回路,缺乏对运动方向、运动速度及驱动区域保持等多维目标的统一协同分配机制,在面对复杂轨迹跟踪任务时,容易出现控制变量耦合严重、动态响应滞后以及计算负荷过大的问题,难以满足长距离、高精度闭环导航的需求。

技术实现思路

1、本发明的目的在于,提供一种机械臂搭载永磁体的磁微纳机器人协同运动控制方法。本发明实现了多维控制变量的解耦协同分配,扩展了有效驱动范围并提高了轨迹跟踪精度。

2、本发明的技术方案:一种机械臂搭载永磁体的磁微纳机器人协同运动控制方法,包括:

3、步骤1、获取磁微纳机器人的当前位置和目标轨迹,并基于所述当前位置和目标轨迹确定磁微纳机器人的期望运动方向和期望运动速度;

4、步骤2、基于运动方向预测模型和所述期望运动方向,确定机械臂末端目标姿态,以调节磁微纳机器人的运动方向;

5、步骤3、基于所述当前位置与预设驱动区域之间的位置关系,确定机械臂末端目标位置,以使磁微纳机器人保持在预设驱动区域内;

6、步骤4、基于运动速度预测模型和所述期望运动速度,确定搭载于机械臂末端的双同步旋转永磁体的目标同步旋转角速度,以调节磁微纳机器人的运动速度;

7、步骤5、基于所述机械臂末端目标姿态、机械臂末端目标位置和目标同步旋转角速度,生成协同控制指令,以实现磁微纳机器人的运动控制。

8、上述的机械臂搭载永磁体的磁微纳机器人协同运动控制方法,基于运动方向预测模型和所述期望运动方向,确定机械臂末端目标姿态,包括:

9、基于所述运动方向预测模型,确定双同步旋转永磁体在当前位置处产生的瞬时总磁场的同频正交分量;

10、基于所述同频正交分量确定局部旋转磁场有向轴,并将所述局部旋转磁场有向轴映射为磁微纳机器人的预测运动方向;

11、以最小化所述预测运动方向与期望运动方向之间的方向误差为目标,求解机械臂末端目标姿态。

12、前述的机械臂搭载永磁体的磁微纳机器人协同运动控制方法,所述同频正交分量包括余弦分量和正弦分量;

13、所述局部旋转磁场有向轴满足以下公式:

14、;

15、所述方向误差满足以下公式:

16、;

17、其中,为局部旋转磁场有向轴,和分别为瞬时总磁场的余弦分量和正弦分量,表示向量叉乘,表示向量范数;为方向误差,为期望运动方向,为预测运动方向。

18、前述的机械臂搭载永磁体的磁微纳机器人协同运动控制方法,基于运动速度预测模型和所述期望运动速度,确定搭载于机械臂末端的双同步旋转永磁体的目标同步旋转角速度,包括:

19、基于所述运动速度预测模型,根据当前位置处的总磁场矢量及其时间导数确定局部磁场角速度;

20、结合磁微纳机器人的旋转动力学模型和旋转-平移推进模型,建立双同步旋转永磁体的同步旋转角速度与预测运动速度之间的映射关系;

21、基于所述映射关系和期望运动速度,反向求解目标同步旋转角速度。

22、前述的机械臂搭载永磁体的磁微纳机器人协同运动控制方法,所述局部磁场角速度满足以下公式:

23、;

24、所述旋转动力学模型满足以下公式:

25、;

26、其中,为局部磁场角速度,为同步旋转角速度,和分别为总磁场矢量在一个同步旋转周期内形成的轨迹椭圆的长轴数值和短轴数值,为总磁场矢量;为等效转动惯量,为磁微纳机器人自转角速度的时间导数,为等效旋转阻尼系数,为自转角速度,为旋转耦合系数。

27、前述的机械臂搭载永磁体的磁微纳机器人协同运动控制方法,基于所述当前位置与预设驱动区域之间的位置关系,确定机械臂末端目标位置,包括:

28、在所述双同步旋转永磁体之间设定优选驱动参考点;

29、基于所述当前位置与优选驱动参考点之间的位置偏差,结合预设空间偏置,确定机械臂末端目标位置,以使优选驱动参考点与当前位置重合或保持预设空间偏置。

30、前述的机械臂搭载永磁体的磁微纳机器人协同运动控制方法,所述确定机械臂末端目标位置,包括:

31、当所述位置偏差的范数大于或等于预设外阈值时,触发机械臂重定位以更新机械臂末端目标位置;

32、当所述位置偏差的范数小于或等于预设内阈值时,停止机械臂平移并保持当前的机械臂末端位置,其中,所述预设内阈值小于预设外阈值;

33、所述机械臂末端目标位置满足以下公式:

34、;

35、其中,为所述机械臂末端目标位置,为所述当前位置,为机械臂末端目标姿态,为优选驱动参考点在机械臂末端坐标系中的位置,为预设空间偏置。

36、前述的机械臂搭载永磁体的磁微纳机器人协同运动控制方法,所述方法还包括:

37、将所述机械臂末端目标姿态、机械臂末端目标位置和目标同步旋转角速度作为联合控制变量;

38、构建包含方向误差、运动速度误差、区域位置误差及控制变化率的联合目标函数;

39、基于所述联合目标函数对联合控制变量进行协同优化,并在优化过程中施加物理约束和安全约束。

40、一种机械臂搭载永磁体的磁微纳机器人协同运动控制系统,包括:

41、机械臂;

42、安装于所述机械臂末端的双同步旋转永磁体磁驱动头;

43、磁微纳机器人定位模块,用于获取磁微纳机器人的当前位置;

44、控制器,与所述机械臂、双同步旋转永磁体磁驱动头以及磁微纳机器人定位模块通信连接,所述控制器被配置为执行如前述的方法。

45、一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现如前述的方法。

46、与现有技术相比,本发明具有以下有益效果:

47、本发明通过将双同步旋转永磁体搭载于机械臂末端,构建了“姿态调向-位置跟随-转速调速”的三维解耦协同控制架构。利用机械臂末端姿态调节局部旋转磁场有向轴以控制运动方向,利用机械臂末端平移维持磁微纳机器人在预设驱动区域内以扩展有效工作空间,利用双永磁体同步旋转角速度调节局部磁场角速度以控制运动速度。该架构基于运动方向预测模型和运动速度预测模型分别反求机械臂目标姿态与磁体目标转速,实现了多维控制变量的独立映射与协同分配,相较于传统固定式驱动或简单级联控制方式,能够在长距离轨迹跟踪过程中持续保持优选驱动条件,提高了磁微纳机器人运动控制的稳定性与轨迹跟踪精度。

48、进一步地,本发明通过引入包含方向误差、速度误差、区域位置误差及控制变化率的联合目标函数,并将机械臂位姿与磁体转速作为联合控制变量进行协同优化,能够在满足机械臂关节限制、步出频率约束及安全距离约束的前提下,平衡方向跟踪、速度跟踪与驱动区域保持之间的性能权衡,同时抑制控制量的剧烈突变,增强了系统在复杂约束环境下的鲁棒性与平滑性。