基于双闭环三维路径跟踪的微机器人磁驱装置和控制方法

技术领域

本发明涉及微纳机器人控制技术领域，尤其是基于双闭环三维路径跟踪的微机器人磁驱装置和控制方法。

背景技术

机器人的小型化为复杂和危险环境下的多种应用提供了解决方案，电磁驱动的磁性微机器人可以应用于医学环境，在不伤害人体的情况下磁性微机器人可以用于携带运输药物实现微创治疗。为了执行这样的任务磁性微机器人应该能实现对期望路径的跟踪。现有磁性微机器人的路径跟踪控制大多应用于平面范围，而且多采用单闭环路径跟踪即通过摄像机的视觉反馈获取磁性微机器在平面内的位姿信息建立位置闭环实现平面范围内的跟踪，然而这其中存在一定的响应延迟和适应性低的问题。一方面在高频环境下由于磁场发生装置的响应延迟问题，所使用的驱动装置往往不能及时准确的建立所需磁场造成响应速度慢。另一方面对复杂的人体环境我们更希望的是磁性微机器人能够完成对三维路径的跟踪，在平面路径跟踪控制领域上述方法可行，但应用于三维环境下的路径跟踪上述的控制方法便失去作用。

发明内容

本发明人针对上述问题及技术需求，提出了基于双闭环三维路径跟踪的微机器人磁驱装置和控制方法，实现对给定的三维期望跟踪路径准确及时的跟踪，克服在高频条件下磁驱装置响应速度慢的问题，采用位置反馈和电流反馈的双闭环控制提高了三维路径跟踪的准确性和快速性。

本发明的技术方案如下：

一种基于双闭环三维路径跟踪的微机器人磁驱装置，包括电磁线圈模组、直流电流源模组、PWM逆变电路、电流传感器、上位机、两个摄像头，电磁线圈模组包括六个带梯形探头的一级铁芯、设置在一级铁芯上的亥姆霍兹线圈以及线圈支架，每对带梯形探头的一级铁芯及其相应的亥姆霍兹线圈均平行设置，线圈支架用于固定三对带梯形探头的一级铁芯和三对亥姆霍兹线圈，三对带梯形探头的一级铁芯沿轴线方向两两正交，三对亥姆霍兹线圈沿轴线方向两两正交，三对梯形探头内侧形成的区域作为磁性微机器人的工作空间，每个直流电流源通过PWM逆变电路后为一对或一个亥姆霍兹线圈提供交流电，上位机分别连接PWM逆变电路、电流传感器和摄像头，电流传感器用于检测亥姆霍兹线圈的输出电流，两个摄像头设置在线圈支架外侧并且正交分布，上位机发送控制信号到PWM逆变电路输出频率和幅值可调的交流电，亥姆霍兹线圈产生旋转磁场进而控制磁性微机器人沿着旋转磁场的轴线方向进行三维运动，摄像头获取磁性微机器人的位置信息并传送至上位机实现闭环控制磁性微机器人的三维运动。

其进一步的技术方案为，线圈支架包括上下相对设置的底座、支架和中空挡板，每个底座沿轴线方向上均设有三个三角斜块，轴向上的一对三角斜块的斜面平行设置且用于放置带梯形探头的一级铁芯，支架设置在底座之间起到支撑作用，中空挡板设置在支架的中部并与底座平行，中空挡板将底座围成的空间分成上层区域和下层区域，每对带梯形探头的一级铁芯及其相应的亥姆霍兹线圈中的一个设置在中空挡板上且位于上层区域、另一个位于下层区域，中空挡板的中空面积至少与磁性微机器人的工作空间面积相同。

其进一步的技术方案为，一级铁芯为DT4-E材料制成的圆柱铁芯，直径50mm、厚30mm，亥姆霍兹线圈的匝数为190，梯形探头的末端为边长35mm的正方形，前端为长16mm、宽2mm的矩形，工作空间为半径16mm的球形空间，磁性微机器人为螺旋形状。

基于双闭环三维路径跟踪的微机器人控制方法，控制方法包括：

在上位机中输入期望跟踪路径得到期望运动方向，摄像机获取磁性微机器人的当前位姿信息并反馈给上位机得到磁性微机器人的质心三维坐标和实际轴线方向；

在期望跟踪路径上寻找与质心距离最小的期望位置点，建立期望跟踪路径曲线坐标系获取期望位置点三维坐标、期望跟踪路径曲线的曲率和挠率；

将实际轴线方向与期望运动方向、质心三维坐标与期望位置点三维坐标分别进行处理得到方向误差和位置误差，方向误差包括俯仰角误差和方向角误差，位置误差包括两者的水平距离和垂直距离；

根据曲率、挠率、方向误差和位置误差对期望跟踪路径的三维运动学建模得到状态空间方程；

通过状态空间方程设计位置闭环控制器得到所需旋转磁场，将旋转磁场输入至建立的电流闭环磁场控制器输出期望磁场，实现对磁性微机器人的双闭环三维运动控制；

重新执行摄像机获取磁性微机器人的当前位姿信息并反馈给上位机得到磁性微机器人的质心三维坐标和实际轴线方向的步骤，直至完成整个期望跟踪路径的跟踪。

其进一步的技术方案为，摄像机获取磁性微机器人的当前位姿信息并反馈给上位机得到磁性微机器人的质心三维坐标和实际轴线方向，包括：

利用侧面摄像机和顶部摄像机对磁性微机器人的当前位姿信息进行检测，把顶部摄像机坐标系固定为世界坐标系，通过下式计算磁性微机器人在世界坐标系中的实际轴线方向：

X

其中，

通过下式计算磁性微机器人的质心在世界坐标系中的三维坐标：

G＝

其中，

其进一步的技术方案为，状态空间方程为：

其中，s表示期望位置点三维坐标，v表示实际运动速度，θ

其进一步的技术方案为，通过状态空间方程设计位置闭环控制器得到所需旋转磁场，包括：

根据链式法则和路径跟踪任务对状态空间方程做线性化处理得到位置闭环控制器的输入量为：

其中，k

路径跟踪任务为使方向角误差、俯仰角误差、垂直距离和水平距离收敛至零；

位置闭环控制器的输出量为：

其中，γ

通过下式计算所需旋转磁场为：

B

B

B＝B

其中，B

其进一步的技术方案为，将旋转磁场输入至建立的电流闭环磁场控制器输出期望磁场，包括：

电流闭环磁场控制器对旋转磁场的方向和电流传感器反馈的线圈输出电流进行分析输出控制信号给PWM逆变电路，PWM逆变电路输出期望电流至亥姆霍兹线圈从而生成期望磁场；

其中，B

将磁场分解到X、Y、Z三轴，则各轴之间的磁场与电流的映射关系如下式：

其中，μ

其进一步的技术方案为，在无扰动状况下磁性微机器人的期望运动方向为其轴线方向，期望运动方向由第一方向角和第一俯仰角表示，第一方向角为向量v

θ

其中，θ

则方向角误差为：θ

俯仰角误差为：θ

运行的磁性微机器人的实际运动速度为：

其中，v

其进一步的技术方案为，期望跟踪路径曲线坐标系是以期望位置点为原点、期望位置点的切线方向、主法线方向、次法线方向为坐标轴建立得到的，切线方向为期望运动方向，利用下述公式获取磁性微机器人的实际轴线方向与第一方向角、第一俯仰角之间的转换关系为：

本发明的有益技术效果是：

本申请提供的磁驱装置能够产生旋转磁场，实现了磁性微机器人在三维空间任意方向的控制，摄像机和集成在上位机中的位置闭环控制器、电流闭环磁场控制器配合实时监测和控制磁性微机器人的运动，PWM逆变电路能够缩短期望磁场的建立时间且使期望磁场更加稳定，上位机能快速给出期望位置点坐标以及磁性微机器人的运行轨迹信息；电流闭环磁场控制器、PWM逆变电路、亥姆霍兹线圈和电流传感器形成第一重闭环控制，在第一重闭环控制中，电流闭环磁场控制器对旋转磁场的方向和电流传感器反馈的输出电流进行分析输出控制信号给PWM逆变电路，PWM逆变电路输出期望电流至亥姆霍兹线圈从而生成期望磁场，位置闭环控制器、电流闭环磁场控制器、PWM逆变电路、亥姆霍兹线圈、磁性微机器人和摄像机形成第二重闭环控制，在第二重闭环控制中，根据摄像机反馈的位姿信息计算与期望运动路径的误差来设计位置闭环控制器得到所需旋转磁场，通过双闭环控制方法实现了磁性微机器人在三维环境下的准确、快速的路径跟踪。

附图说明

图1是本申请提供的磁驱装置原理图。

图2是本申请提供的电磁线圈模组整体装配正视图。

图3是本申请提供的去掉一个底座和支架的电磁线圈模组装配图。

图4是本申请提供的带三角斜块的底座示意图。

图5是本申请提供梯形探头、一级铁芯和二级铁芯的组合三视图。

图6是本申请提供的控制方法流程图。

图7是本申请提供的磁性微机器人在三维空间下运动的角度补偿示意图。

图8是本申请提供的获取磁性微机器人质心三维坐标和实际轴线方向的原理示意图。

图9是本申请提供的磁性微机器人在跟踪路径曲线坐标系下的期望跟踪路径模型图。

图10是本申请提供的第一重闭环控制的部分电路图。

具体实施方式

下面结合附图对本发明的具体实施方式做进一步说明。

本申请公开了一种基于双闭环三维路径跟踪的微机器人磁驱装置，图1示出了磁驱装置的原理图，磁驱装置包括电磁线圈模组、直流电流源模组、PWM逆变电路、电流传感器、上位机、两个摄像头。PWM逆变电路能够缩短期望磁场的建立时间且使期望磁场更加稳定，上位机能快速给出期望位置点坐标以及磁性微机器人的运行轨迹信息。结合图2-图4所示，电磁线圈模组包括六个带梯形探头1的一级铁芯2、设置在一级铁芯2上的亥姆霍兹线圈3以及线圈支架，每对带梯形探头1的一级铁芯2及其相应的亥姆霍兹线圈3均平行设置，线圈支架用于固定三对带梯形探头1的一级铁芯2和三对亥姆霍兹线圈3，具体的，线圈支架包括上下相对设置的底座401、支架402和中空挡板403，每个底座401沿轴线方向上均设有三个三角斜块404，轴向上的一对三角斜块404的斜面平行设置且用于放置带梯形探头1的一级铁芯2，支架402设置在底座401之间起到支撑作用，中空挡板403设置在支架402的中部并与底座401平行，中空挡板403将底座401围成的空间分成上层区域和下层区域，每对带梯形探头1的一级铁芯2及其相应的亥姆霍兹线圈3中的一个设置在中空挡板403上且位于上层区域、另一个位于下层区域。

三对带梯形探头1的一级铁芯2沿轴线方向两两正交，三对亥姆霍兹线圈3沿轴线方向两两正交，三对梯形探头1内侧形成的区域作为磁性微机器人的工作空间，中空挡板403的中空面积至少与磁性微机器人的工作空间面积相同。每个直流电流源通过PWM逆变电路后为一对或一个亥姆霍兹线圈3提供交流电，也即本申请的直流电流源至少为六个，至多为十二个。上位机分别连接PWM逆变电路、电流传感器和摄像头，电流传感器用于检测亥姆霍兹线圈3的输出电流，两个摄像头设置在线圈支架402外侧并且正交分布，上位机发送控制信号到PWM逆变电路输出频率和幅值可调的交流电，亥姆霍兹线圈3产生旋转磁场进而控制磁性微机器人沿着旋转磁场的轴线方向进行三维运动，摄像头获取磁性微机器人的位置信息并传送至上位机实现闭环控制磁性微机器人的三维运动。

可选的，电磁线圈模组还包括六个二级铁芯5以及围绕其设置的梯形线圈6，二级铁芯5设置在未设有梯形探头1的一级铁芯2的一侧上，同理，每对二级铁芯5平行设置且放置在三角斜块404上，三对二级铁芯5沿轴线方向两两正交，三对梯形线圈6沿轴线方向两两正交，梯形探头1、一级铁芯2和二级铁芯5呈阶梯状排列。

可选的，如图5所示，图5(a)为探头和铁芯组合的正视图，图5(b)为探头和铁芯组合的侧视图，图5(c)为探头和铁芯组合的俯视图。一级铁芯2和二级铁芯5均为DT4-E材料制成的圆柱铁芯，一级铁芯2的直径50mm、厚30mm，亥姆霍兹线圈3的匝数为190，二级铁芯5的直径70mm、厚35mm，梯形线圈6的匝数为610，对应的两个线圈串联通电，梯形探头1的末端为边长35mm的正方形，前端为长16mm、宽2mm的矩形。

可选的，根据上述带梯形探头1的一级铁芯2围成的工作空间为半径16mm的球形空间，磁性微机器人如图7所示为螺旋形状。

本申请还公开了一种基于双闭环三维路径跟踪的微机器人控制方法，能够应用在上述的磁驱装置中，该控制方法的流程图如图6所示，包括如下步骤：

步骤1：在上位机中输入期望跟踪路径得到期望运动方向，摄像机获取磁性微机器人的当前位姿信息并反馈给上位机得到磁性微机器人的质心三维坐标和实际轴线方向。

如图7所示，在无扰动状况下磁性微机器人的期望运动方向为其轴线方向，期望运动方向由第一方向角和第一俯仰角表示，第一方向角为向量v

θ

其中，θ

运行的磁性微机器人的实际运动速度为：

其中，v

摄像机获取磁性微机器人的当前位姿信息并反馈给上位机得到磁性微机器人的质心三维坐标和实际轴线方向，如图8所示，包括：

利用侧面摄像机和顶部摄像机对磁性微机器人的当前位姿信息进行检测，把顶部摄像机坐标系固定为世界坐标系，通过下式计算磁性微机器人在世界坐标系中的实际轴线方向：

X

其中，

通过下式计算磁性微机器人的质心在世界坐标系中的三维坐标：

G＝

其中，

步骤2：在期望跟踪路径上寻找与质心距离最小的期望位置点，建立期望跟踪路径曲线坐标系获取期望位置点三维坐标、期望跟踪路径曲线的曲率和挠率。

如图9所示，期望跟踪路径曲线坐标系是以期望位置点s为原点、期望位置点的切线方向X

步骤3：将实际轴线方向与期望运动方向、质心三维坐标与期望位置点三维坐标分别进行处理得到方向误差和位置误差，方向误差包括俯仰角误差和方向角误差，位置误差包括两者的水平距离和垂直距离。

则方向角误差为：θ

俯仰角误差为：θ

水平距离d

步骤4：根据曲率、挠率、方向误差和位置误差对期望跟踪路径的三维运动学建模得到状态空间方程。

则状态空间方程为：

其中，s表示期望位置点三维坐标，v表示实际运动速度，θ

步骤5：通过状态空间方程设计位置闭环控制器得到所需旋转磁场，将旋转磁场输入至建立的电流闭环磁场控制器输出期望磁场，实现对磁性微机器人的双闭环三维运动控制。

根据链式法则和路径跟踪任务对状态空间方程做线性化处理得到位置闭环控制器的输入量为：

其中，k

路径跟踪任务为使所述方向角误差、俯仰角误差、垂直距离和水平距离收敛至零。

位置闭环控制器的输出量为：

其中，γ

通过下式计算所需旋转磁场为：

B

B

B＝B

其中，B

图10示出了第一重闭环控制的部分电路图，电流闭环磁场控制器对旋转磁场的方向和电流传感器反馈的线圈输出电流进行分析输出PWM控制信号给PWM逆变电路，PWM逆变电路输出期望电流至亥姆霍兹线圈3从而生成期望磁场。

其中，B

将磁场分解到X、Y、Z三轴，则各轴之间的磁场与电流的映射关系如下式：

其中，μ

步骤6：重新执行摄像机获取磁性微机器人的当前位姿信息并反馈给上位机得到磁性微机器人的质心三维坐标和实际轴线方向的步骤，直至完成整个期望跟踪路径的跟踪。

在本申请中，在上位机中设计电流闭环磁场控制器和位置闭环控制器，电流闭环磁场控制器、PWM逆变电路、亥姆霍兹线圈和电流传感器形成第一重闭环控制，在第一重闭环控制中，电流闭环磁场控制器输出控制信号给PWM逆变电路，PWM逆变电路输出期望电流至亥姆霍兹线圈从而生成期望磁场；位置闭环控制器、电流闭环磁场控制器、PWM逆变电路、亥姆霍兹线圈、磁性微机器人和摄像机形成第二重闭环控制，在第二重闭环控制中，根据摄像机反馈的位姿信息计算与期望运动路径的误差来设计位置闭环控制器得到所需旋转磁场，通过双闭环控制方法实现了磁性微机器人在三维环境下的准确、快速的路径跟踪。

以上所述的仅是本申请的优选实施方式，本发明不限于以上实施例。可以理解，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围之内。