一种磁性微纳机器人的自主导航与控制方法与流程

1.本发明涉及微纳机器人技术领域，尤其是一种磁性微纳机器人的自主导航与控制方法。


背景技术：

2.近年来，磁场驱动式的微纳机器人由于远程化、入侵性小等优势，正受到广泛的关注，许多团体开始研究其在生物医疗中的应用，但是微纳机器人自主导航与控制系统在实际的应用中还是存在着不少的挑战。
3.以往的研究中，在对微纳机器人进行路径规划时，并未考虑运动对象的运动约束。如果规划的路径过于曲折，将会给机器人的运动控制带来很大的麻烦。另一方面，当前人们普遍采用pid或者滑膜控制器对微纳机器人进行控制，然而在一些复杂的高阶非线性系统中，传统的pid控制方法鲁棒性较差。基于滑膜控制器的方法在一定程度上具有优势，但其控制方法的主要缺点是“抖动”现象，在实际系统中可能会损坏执行器。


技术实现要素：

4.本发明人针对上述问题及技术需求，提出了一种磁性微纳机器人的自主导航与控制方法，通过改进的rrt
‑
connet算法，为磁性微纳机器人规划出平滑易控的最优避障路径，利用神经网络来构建从系统观测和控制输入到控制输出的磁性微纳机器人的轨迹跟踪控制器，弥补传统控制算法鲁棒性差，易产生抖动的缺点。
5.本发明的技术方案如下：
6.一种磁性微纳机器人的自主导航与控制方法，磁性微纳机器人的自主导航与控制系统包括形成闭环控制的交流电流源、三轴亥姆霍兹线圈、磁性微纳机器人、显微镜和上位机；
7.方法包括如下步骤：
8.建立磁性微纳机器人的车辆等效模型，将对磁性微纳机器人的控制等效为对微车的转向角和油门的控制；
9.对显微镜采集的图像进行图像特征的提取、匹配和融合，从局部地图扩充到全局地图，并利用语义分割和目标检测方法获取磁性微纳机器人的状态信息和所处环境信息；
10.利用改进的rrt
‑
connet算法，引入边界扩充和曲线平滑算法对原始轨迹进行处理，得到磁性微纳机器人在全局地图的最优避障路径；
11.基于等效模型和状态信息，对磁性微纳机器人进行运动学建模得到状态空间方程；
12.利用状态空间方程设计基于强化学习的轨迹跟踪控制器，轨迹跟踪控制器依据状态信息输出控制信号至交流电流源，从而改变空间旋转磁场以跟踪最优避障路径，实现对磁性微纳机器人的闭环控制。
13.其进一步的技术方案为，利用改进的rrt
‑
connet算法，引入边界扩充和曲线平滑
算法对原始轨迹进行处理，得到磁性微纳机器人在全局地图的最优避障路径，包括：
14.通过像素语义图计算磁性微纳机器人的设定工作环境的可驱动区域的边界，然后根据磁性微纳机器人在像素上的大小扩展可驱动区域的边界，将扩展结果以外的区域作为非驾驶区，可驱动区域边界内部为驾驶区；
15.通过rrt
‑
connet算法对规划路径进行节点细化，以提取关键地标，然后使用b样条函数优化由关键地标形成的局部路径以产生具有连续曲率的平滑路径作为最优避障路径。
16.其进一步的技术方案为，基于磁性微纳机器人的运动状态，定义状态空间方程为：
[0017][0018]
其中，δ表示微车的转向角，τ表示微车的油门；e
θ
表示微车与最优避障路径之间的角度差，是一阶导数；e
x
、e
y
分别是x和y轴方向的位置误差，和是相应的一阶导数，和是相应的二阶导数，导数用于向轨迹跟踪控制器提供时间信息；χ中包含在最优避障路径中当前位置的后两个路径点的二维坐标，s表示维度为14的状态空间方程。
[0019]
其进一步的技术方案为，利用状态空间方程设计基于强化学习的轨迹跟踪控制器，包括：
[0020]
基于强化学习的轨迹跟踪控制器由三种神经网络组成，包括两个软q函数q
θ1
(s
t
,a
t
)、q
θ2
(s
t
,a
t
)和可处理策略其中，s
t
表示t时刻状态空间方程的状态信息，a
t
表示t时刻的动作空间，三种神经网络的参数是θ
1、2
和φ0；
[0021]
算法流程如下：首先，观察当前时刻状态空间方程的14维状态，然后通过策略网络将状态信息转移到具有完全连接层的2维动作；从输出分布中采样动作，并使用tanh激活函数将其归一化为[
‑
1,1]，采样动作将被进一步映射和平滑以与环境交互；当获得下一个状态信息s
t 1
和r(s
t
,a
t
)时，参数(s
t
,a
t
,r(s
t
,a
t
),s
t 1
)被存储到缓冲区中，在训练过程中会重复进行上述交互和存储的过程；在训练结束时，当过渡次数大于设置阈值时，分别使用函数j
q
(θ
i
)和j
π
(φ0)更新网络，重复整个算法流程，直到得到最佳策略为止。
[0022]
其进一步的技术方案为，轨迹跟踪控制器依据状态信息输出控制信号至交流电流源，从而改变空间旋转磁场以跟踪最优避障路径，包括：
[0023]
向轨迹跟踪控制器输入当前时刻的状态空间方程，输出包括转向角和油门控制量的动作空间；
[0024]
将动作空间等效为空间旋转磁场的旋转角度，对当前时刻的空间旋转磁场值进行磁场转换，得到下一时刻旋转后的空间旋转磁场值；
[0025]
根据下一时刻旋转后的空间旋转磁场值计算得到线圈的三轴电流，进而转换为交流电对应的频率和幅值作为控制信号输入至交流电流源。
[0026]
其进一步的技术方案为，将动作空间等效为空间旋转磁场的旋转角度，对当前时刻的空间旋转磁场值进行磁场转换，得到下一时刻旋转后的空间旋转磁场值，包括：
[0027]
采用四元数方法推导空间旋转磁场的产生，对于空间磁场矢量om，o:(0,0,0)表示在空间的原点，设当前时刻的空间旋转磁场值m为(x,y,z)，下一时刻旋转后的空间旋转磁场值m’为(x’,y’,z’)；空间三维旋转视为绕三个基本轴的旋转组合，绕三个基本轴的旋转角度是其中，将旋转矢量的旋转角度等效为微车的转向角控制量，将旋转矢量的
旋转角度φ等效为微车的油门控制量，且φ＝∫ωtdt，ω表示旋转角速度；
[0028]
旋转前四元数坐标m[0(x,y,z)]是通过四元数方法展开磁场值(x,y,z)后得到的，求解旋转后四元数坐标m’[0(x’,y’,z’)]的过程如下：
[0029]
q＝[q0(q
1 q
2 q3)]
[0030]
q
‑1＝[q0(
‑
q1ꢀ‑
q2ꢀ‑
q3)]
[0031]
其中，q代表单位四元数；
[0032]
设三个基本轴的旋转顺序依次为z、y、x，则旋转过程定义为：
[0033][0034]
qm＝[q0(q
1 q
2 q3)][0(x,y,z)]＝[w1(v1,v2,v3)]
[0035]
旋转后四元数坐标m’基于单位四元数与旋转的关系，定义为：
[0036]
m
′
＝qmq
‑1＝[w1(v1,v2,v3)][q0(
‑
q1‑
q2‑
q3)]＝[w0(u,v,w)]
[0037]
其中，中间变量分别表示为：
[0038][0039][0040]
则下一时刻旋转后的空间旋转磁场值m’表示为：
[0041][0042]
其进一步的技术方案为，根据下一时刻旋转后的空间旋转磁场值计算得到线圈的三轴电流，其中，计算公式为：
[0043][0044]
其中，b
e
记为空间旋转磁场，μ0表示空间中的磁导率，n
k
(k＝x,y,z)表示三轴亥姆
霍兹线圈中每个线圈的匝数，i
k
(k＝x,y,z)表示三轴电流，a
k
(k＝x,y,z)表示三轴亥姆霍兹线圈中每个线圈的半径；
[0045]
通过空间旋转磁场与机器人的磁矩的乘积产生的磁转矩来操纵磁性微纳机器人按照最优避障路径运动，磁转矩的表达式为：t
e
＝m
×
b
e
，其中m表示磁矩。
[0046]
其进一步的技术方案为，方法还包括：
[0047]
基于最优避障路径跟随和非冲突的目标，设计回报函数用来评估轨迹跟踪控制器的性能，设计回报函数r
eθ
、r
ex
、r
ey
和r
co
，表示如下：
[0048][0049][0050][0051]
其中，r
eθ
、r
ex
、r
ey
表示距离目标点误差的回报函数，实现目标跟踪和自主规划功能；r
co
表示机器人避开非驾驶区的回报函数，对静态避障和动态避障同时有效，以磁性微纳机器人的直径作为圆的半径，圆的面积为非驾驶区与圆相交的面积为s
co
，则总回报累积为：
[0052]
r＝∑(r
eθ
(t) r
ex
(t) r
ey
(t) r
co
(t))。
[0053]
其进一步的技术方案为，磁性微纳机器人采用磁珠，材料为钕铁硼材料、半径为r
m
，磁性微纳机器人在设定工作环境中运动，设定工作环境为含有流体的微流控芯片或者培养皿。
[0054]
其进一步的技术方案为，分析磁性微纳机器人在设定工作环境下的滚动条件，包括：
[0055]
在空间旋转磁场下，磁性微纳机器人受到磁转矩t
e
，并将磁转矩t
e
表示为切向力f
t
，磁性微纳机器人还受到地面摩擦力、流体阻力f
p
、浮力f
b
和自身重力g；
[0056]
其中：f
t
＝t
e
/r
m
[0057]
f
s
＝μ(g
‑
f
b
)
[0058]
f
r
＝σ(g
‑
f
b
)
[0059]
f
s
为静摩擦力，f
r
为滚动摩擦力，μ和σ分别为静摩擦系数和滚动摩擦系数；
[0060]
则磁性微纳机器人的滚动条件为：
[0061][0062]
本发明的有益技术效果是：
[0063]
通过对磁性微纳机器人进行车辆等效建模，大大优化了复杂环境下磁性微纳机器人的控制策略，采用闭环控制能够实时监测磁性微纳机器人的运动，通过改进的rrt
‑
connet算法，为磁性微纳机器人规划出平滑易控的最优避障路径，设计基于强化学习的轨迹跟踪控制器，弥补了传统算法鲁棒性差，易产生抖动的缺点，在上位机中将控制器输出的动作空间通过磁场转换为下一时刻旋转后的空间旋转磁场值，进而转换为交流电对应的频
率和幅值作为控制信号输入至交流电流源，实现改变空间旋转磁场以跟踪最优避障路径；依据机器人的运动约束设计了回报函数以指导强化学习网络的收敛方向，提高了磁性微纳机器人的轨迹跟踪控制精度。
附图说明
[0064]
图1是本技术提供的磁性微纳机器人的自主导航与控制系统的闭环控制图。
[0065]
图2是本技术提供的磁性微纳机器人的自主导航与控制方法的流程图。
[0066]
图3是本技术提供的磁性微纳机器人的受力分析图。
[0067]
图4是本技术提供的磁性微纳机器人的等效模型图。
[0068]
图5是改进的rrt
‑
connet算法的效果示意图，其中：(a)是标准rrt算法的效果图，(b)是边界扩充后的设定工作环境的示意图，(c)是边界扩充后的路径规划效果图，(d)是b样条函数平滑后的路径规划效果图。
[0069]
图6是本技术提供的基于强化学习的轨迹跟踪控制器的网络架构。
具体实施方式
[0070]
下面结合附图对本发明的具体实施方式做进一步说明。
[0071]
如图1所示，磁性微纳机器人的自主导航与控制系统包括形成闭环控制的交流电流源、三轴亥姆霍兹线圈、磁性微纳机器人、显微镜和上位机。交流电流源包括直流电源和驱动板，直流电源通过驱动板将直流电转换为交流电，为三轴亥姆霍兹线圈提供交流电、以产生空间旋转磁场驱动磁性微纳机器人运动。上位机分别与驱动板、显微镜进行数据通信，显微镜用于获取磁性微纳机器人的状态信息和所处环境信息，给上位机提供视觉反馈信号。上位机中的路径规划器依据状态信息和环境信息进行路径规划，上位机中的轨迹跟踪控制器依据状态信息输出控制信号至驱动板上输出频率和幅值可调的交流电，从而改变空间旋转磁场以跟踪最优避障路径。
[0072]
可选的，本技术采用磁珠作为磁性微纳机器人，材料为钕铁硼材料、半径为r
m
，磁性微纳机器人在设定工作环境中运动，设定工作环境为含有流体的微流控芯片或者培养皿。
[0073]
基于上述系统，本技术还提供了一种磁性微纳机器人的自主导航与控制方法，如图2所示，具体包括如下步骤：
[0074]
步骤1：建立磁性微纳机器人的车辆等效模型。
[0075]
首先，分析磁性微纳机器人在设定工作环境下的滚动条件，包括：
[0076]
如图3所示，在空间旋转磁场下，磁性微纳机器人受到磁转矩t
e
，并将磁转矩t
e
表示为切向力f
t
，磁性微纳机器人还受到地面摩擦力、流体阻力f
p
、浮力f
b
和自身重力g；
[0077]
其中：f
t
＝t
e
/r
m
[0078]
f
s
＝μ(g
‑
f
b
)
[0079]
f
r
＝σ(g
‑
f
b
)
[0080]
f
s
为静摩擦力，f
r
为滚动摩擦力，μ和σ分别为静摩擦系数和滚动摩擦系数；
[0081]
则磁性微纳机器人的滚动条件为：
[0082][0083]
进一步的，为了优化复杂工作环境下磁性微纳机器人的控制策略，本技术建立车辆等效模型，将对磁性微纳机器人的控制等效为对微车的转向角和油门的控制，如图4所示。
[0084]
步骤2：对显微镜采集的图像进行图像特征的提取、匹配和融合，从局部地图扩充到全局地图，并利用语义分割和目标检测方法获取磁性微纳机器人的状态信息和所处环境信息。
[0085]
步骤3：利用改进的rrt
‑
connet算法，引入边界扩充和曲线平滑算法对原始轨迹进行处理，得到磁性微纳机器人在全局地图的最优避障路径。
[0086]
由于没有运动约束，所以规划的路径就像是树枝分支，而且该路径靠近驾驶区域的边界，因此，微纳机器人在行驶过程中很容易遇到边界，如图5
‑
(a)所示，这将给微车的运动控制带来困难，因此本技术采用改进的rrt
‑
connet算法，可用区域边界的扩展不会在rrt
‑
connet的迭代过程中引入额外的计算，因此对总计划时间的影响很小，具体包括：
[0087]
首先通过像素语义图计算磁性微纳机器人的设定工作环境的可驱动区域的边界，然后根据磁性微纳机器人在像素上的大小扩展可驱动区域的边界，如图5
‑
(b)所示，粗线代表可驱动区域边界扩展的结果，将扩展结果以外的区域作为非驾驶区1，可驱动区域边界内部为驾驶区2。在新的语义图(b)上进行了模拟，效果图如图5
‑
(c)所示，可以看到规划路径3不再出现在边界附近。
[0088]
进一步，通过rrt
‑
connet算法对规划路径进行节点细化，以提取关键地标，然后使用b样条函数优化由关键地标形成的局部路径以产生具有连续曲率的平滑路径作为最优避障路径4，如图5
‑
(d)所示。
[0089]
步骤4：基于等效模型和状态信息，对磁性微纳机器人进行运动学建模得到状态空间方程。
[0090]
基于磁性微纳机器人的运动状态，定义状态空间方程为：
[0091][0092]
其中，δ表示微车的转向角，τ表示微车的油门；e
θ
表示微车与最优避障路径之间的角度差，是一阶导数；e
x
、e
y
分别是x和y轴方向的位置误差，和是相应的一阶导数，和是相应的二阶导数，导数用于向轨迹跟踪控制器提供时间信息；χ中包含在最优避障路径中当前位置的后两个路径点的二维坐标，s表示维度为14的状态空间方程。
[0093]
步骤5：利用状态空间方程设计基于强化学习的轨迹跟踪控制器，轨迹跟踪控制器依据状态信息输出控制信号至交流电流源，从而改变空间旋转磁场以跟踪最优避障路径，实现对磁性微纳机器人的闭环控制，具体包括：
[0094]
<1>如图6所示，基于强化学习的轨迹跟踪控制器由三种神经网络组成，包括两个软q函数q
θ1
(s
t
,a
t
)、q
θ2
(s
t
,a
t
)和可处理策略其中，s
t
表示t时刻状态空间方程的状态信息，a
t
表示t时刻的动作空间，三种神经网络的参数是θ
1、2
和φ0。
[0095]
算法流程如下：首先，观察当前时刻状态空间方程的14维状态，然后通过策略网络
将状态信息转移到具有完全连接层的2维动作；从输出分布中采样动作，并使用tanh激活函数将其归一化为[
‑
1,1]，采样动作将被进一步映射和平滑以与环境交互；当获得下一个状态信息s
t 1
和r(s
t
,a
t
)时，参数(s
t
,a
t
,r(s
t
,a
t
),s
t 1
)被存储到缓冲区中，在训练过程中会重复进行上述交互和存储的过程；在训练结束时，当过渡次数大于设置阈值时，分别使用函数j
q
(θ
i
)和j
π
(φ0)更新网络，重复整个算法流程，直到得到最佳策略为止。
[0096]
<2>向轨迹跟踪控制器输入当前时刻的状态空间方程，输出包括转向角和油门控制量的动作空间。
[0097]
<3>将动作空间等效为空间旋转磁场的旋转角度，对当前时刻的空间旋转磁场值进行磁场转换，得到下一时刻旋转后的空间旋转磁场值，具体包括：
[0098]
采用四元数方法推导空间旋转磁场的产生，对于空间磁场矢量om，o:(0,0,0)表示在空间的原点，设当前时刻的空间旋转磁场值m为(x,y,z)，下一时刻旋转后的空间旋转磁场值m’为(x’,y’,z’)；空间三维旋转视为绕三个基本轴的旋转组合，绕三个基本轴的旋转角度是其中，将旋转矢量的旋转角度等效为微车的转向角控制量，将旋转矢量的旋转角度φ等效为微车的油门控制量，且φ＝∫ωtdt，ω表示旋转角速度；
[0099]
旋转前四元数坐标m[0(x,y,z)]是通过四元数方法展开磁场值(x,y,z)后得到的，求解旋转后四元数坐标m’[0(x’,y’,z’)]的过程如下：
[0100]
q＝[q0(q
1 q
2 q3)]
[0101]
q
‑1＝[q0(
‑
q1ꢀ‑
q2ꢀ‑
q3)]
[0102]
其中，q代表单位四元数；
[0103]
设三个基本轴的旋转顺序依次为z、y、x，则旋转过程定义为：
[0104][0105]
qm＝[q0(q
1 q
2 q3)][0(x,y,z)]＝[w1(v1,v2,v3)]
[0106]
旋转后四元数坐标m’基于单位四元数与旋转的关系，定义为：
[0107]
m
′
＝qmq
‑1＝[w1(v1,v2,v3)][q0(
‑
q1‑
q2‑
q3)]＝[w0(u,v,w)]
[0108]
其中，中间变量分别表示为：
[0109]
[0110][0111]
则下一时刻旋转后的空间旋转磁场值m’表示为：
[0112][0113]
<4>根据下一时刻旋转后的空间旋转磁场值计算得到线圈的三轴电流，进而转换为交流电对应的频率和幅值作为控制信号输入至交流电流源。
[0114]
计算公式为：
[0115][0116]
其中，b
e
记为空间旋转磁场，μ0表示空间中的磁导率，n
k
(k＝x,y,z)表示三轴亥姆霍兹线圈中每个线圈的匝数，i
k
(k＝x,y,z)表示三轴电流，a
k
(k＝x,y,z)表示三轴亥姆霍兹线圈中每个线圈的半径。
[0117]
通过空间旋转磁场与机器人的磁矩的乘积产生的磁转矩来操纵磁性微纳机器人按照最优避障路径运动，磁转矩的表达式为：t
e
＝m
×
b
e
，其中m表示磁矩。
[0118]
步骤6：基于最优避障路径跟随和非冲突的目标，设计回报函数用来评估轨迹跟踪控制器的性能，设计回报函数r
eθ
、r
ex
、r
ey
和r
co
，表示如下：
[0119][0120][0121][0122]
其中，r
eθ
、r
ex
、r
ey
表示距离目标点误差的回报函数，实现目标跟踪和自主规划功能；r
co
表示机器人避开非驾驶区的回报函数，对静态避障和动态避障同时有效，以磁性微纳机器人的直径作为圆的半径，圆的面积为非驾驶区与圆相交的面积为s
co
，则总回报累积为：
[0123]
r＝∑(r
eθ
(t) r
ex
(t) r
ey
(t) r
co
(t))。
[0124]
对轨迹跟踪控制器进行模型训练的目的是使总回报越高越好。
[0125]
以上所述的仅是本技术的优选实施方式，本发明不限于以上实施例。可以理解，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围之内。