一种核工业检测机器人的任务规划方法

1.本发明属于核工业机器人任务规划技术领域。


背景技术：

2.核电因其环保、清洁和相对安全的特点被广泛应用，目前核电年发电量已经占世界发电总量的10％以上。连接核电站一回路和二回路之间的热交换器是蒸汽发生器，一回路的水在蒸汽发生器的传热管中加热至沸腾，水蒸气送到汽轮机，为涡轮发电机提供动力。每个蒸汽发生器包含数千根管道，这些管道固定在半圆形管板上，为了保证热传递效率，传热管的管壁厚度只有1mm～1.2mm，是蒸汽发生器内部工作最薄弱的环节，因此需要定期对传热管进行检修。蒸汽发生器内部的放射性极高，需要操纵机器人在管板上运动并定位至对应传热管，将检修工具送至传热管内部进行检修。目前对蒸汽发生器内的检查机器人研究多集中于机器人结构，对检查算法的研究较少。由于管板尺寸较大，传热管数量较多，检修任务可能会包含大量分布在管板上不同位置的传热管，而每根传热管的检修时间长达2分钟，为提高检修效率，移动机器人一般在作业臂末端携带两个工具同时进行检修。检修时机器人沿着管板的分布方向遍历运动，目前所采用的主要算法是按行和按列遍历搜索算法，使机器人沿着管板行和管板的列方向进行遍历检查。但是，这两种检查方式都会导致检查大量重复或不在任务范围内的传热管，增加核电站停堆时间。


技术实现要素：

3.本发明是为了解决现有的检查方式都会导致检查大量重复或不在任务范围内的传热管问题，以及导致机器人的运动时间长的问题。
4.一种核工业检测机器人的任务规划方法，包括以下步骤：
5.步骤1、根据机器人与工具的结构及参数，在机器人工具的工作空间内求解工具的工作区域；
6.步骤2、首先对给定的任务区域按照矩形区域进行补全，并对任务区域进行合并；针对所有补全、合并后的矩形区域，确定最优工作宽度；根据各矩形区域的最优工作方向及其对应的工作宽度将矩形区域进一步划分成实际工作的子区域；
7.步骤3、基于搜索方向，根据最优工作方向、次优工作方向和沿长度方向的工作方向整体求解机器人工作位置点；
8.步骤4、利用蚁群算法规划工作位置点全局遍历路径和各个任务的作业顺序；
9.当给定多个需要检查的任务时，任务规划算法根据上述规划，为每个任务分配机器人位置和对应的关节配置。
10.进一步地，所述步骤1的具体过程包括以下步骤：
11.机器人是一个三自由度的平面运动机器人，根据机器人的移动自由度和两个旋转自由度确定机器人末端设置的两个工具所能到达的所有空间，即末端工具的工作空间；如果机器人在某个位置上能使末端工具扫查到工作空间中给定的任务管孔，则称该位置为该
给定任务管孔对应的机器人工作位置点；
12.将工具的工作空间内不同长度、宽度的矩形记为工作区域，将工作区域的宽度称为工具的工作宽度；
13.将两个工具间隔的k个管孔，工具的最优检查方式是两个工具均能扫查，即某个矩形区域的宽度能保证工具以间隔k个管孔的方式沿着该宽度扫查完一列，满足上述条件的工作宽度作为最小最优工作宽度，即最小最优工作宽度为2k；最优工作宽度{{2k},{2*2k},{3*2k},
…
}；同理，当某个矩形区域的宽度保证工具间隔k个管孔沿着该宽度检查一列管孔时，部分的管孔均能被两个工具同时检查，只多检查1个不在对应矩形区域的管孔，将满足这种条件的矩形宽度称为次优工作宽度{k 1,k 2,
…
2k-1}}；当某个矩形宽度为1时，工具只能沿着对应矩形的长度方向进行单排的扫查，这时工具的工作性能取决于矩形的长度，这种工作方式称为沿着长度方向的扫查，且工作宽度为1；
14.根据最优工作宽度、次优工作宽度和长度方向对应的工作宽度对应的工作宽度所在方向分别记为最优工作方向、次优工作方向和沿长度方向的工作方向；
15.根据最优工作宽度、次优工作宽度和长度方向对应的工作宽度，在工具的工作空间内找到宽度为工作宽度，长度为使某区域在工作空间内部且最长的矩形区域，则该矩形区域的长度称为工作长度；在工具的工具空间内部搜索满足相应工作宽度的区域，记录所有矩形区域的工作长度和工作宽度，即完成了对机器人工作的最优、次优和长度方向对应工作区域的求解。
16.进一步地，所述步骤2的具体过程包括以下步骤：
17.2.1、对给定的任务区域进行补全和合并处理：
18.首先对给定的任务区域按照矩形区域进行补全，即将不规则的任务区域补全为一个最小能包含该规则区域的规则矩形区域。
19.然后，根据三种可选检查方式，确定沿着检查任务区域宽度w方向进行检查的单位任务数量为：
[0020][0021][0022]
其中，为向下取整函数；w
2k
对应的区域沿着最优宽度工作，w
remain
对应的区域沿着长度方向工作，w
k 1-w
2k-1
对应的区域是沿着次优宽度方向工作；机器人对应的检查任务区域宽度为w，区域长度为l的工作区域所需检查任务的总数量就是num(w)
×
l；
[0023]
三种可选检查方式分别是沿着最优或次优工作宽度的方向、沿着长度的方向、最优或次优工作宽度与长度组合的方向对应的检查任务；
[0024]
基于评价函数评价对任务区域进行合并；沿着检查任务区域宽度w方向进行检查的评价函数为：
[0025][0026]
其中，α
2k
为沿宽度方向的工作因子，α
2k
＞0，α
remain
为沿长度方向的工作因子，α
remain
＜0，α
2k-1
,α
2k-2
,
…
α
k 1
为沿着宽度和长度组合方向的工作因子，所有的工作因子均在[0,1]之间取值，利用工作因子为权重即可计算评价函数的值；
[0027]
进一步地，所述合并过程包括以下步骤：
[0028]
针对相互靠近且同一方向的尺寸相同的p个任务区域，用一个宽/长为a的矩形区域覆盖p个宽/长为a1,a2,
…
,ap的矩形任务区域，且a＝a1＝a2＝
…
＝ap；p个任务区域合并后对合并区域进行判断，如果合并区域的评价函数值大于等于每个任务区域评价函数值的和，则保留该合并区域，否则保留原区域。
[0029]
2.2、完成区域的补全和合并之后，针对所有补全、合并后的矩形区域，根据评价函数求解方式，计算各矩形区域沿着长度和宽度工作的评价函数，函数值最大即为主要工作宽度，根据主要工作宽度确定最优工作宽度；根据各矩形区域的最优工作方向及其对应的工作宽度将矩形区域进一步划分成实际工作的子区域。
[0030]
进一步地，所述步骤3的具体过程包括以下步骤：
[0031]
3.1、确定工作位置的搜索方向；
[0032]
进一步地，工作位置的搜索方向如下：
[0033]
以矩形区域沿着宽度方向的两边界距离机器人当前工作位置的代价作为基本判断标准，选取从远到近的搜索方式；
[0034]
3.2、根据最优工作方向、次优工作方向和沿长度方向的工作方向整体求解机器人工作位置点，包括以下步骤：
[0035]
3.2.1、判断整体搜索的长度；
[0036]
工作长度指给定工作宽度时使工作区域在工作空间内部且最长的矩形区域长度；
[0037]
根据最优工作方向、次优工作方向和沿长度方向的工作方向，查找待搜索区域中某个子区域工作宽度的最大工作长度，将所有未搜索子区域的工作长度与工具工作空间内对应的最大工作长度对比，选取其中较小的进行整体搜索；
[0038]
3.2.2、选取整体搜索的关键任务，通过搜索关键任务得到待搜索子区域的关键位置；
[0039]
进一步地，在搜索关键任务时既要选择一个子区域两侧的任务，还要选择子区域中心的任务作为关键任务。
[0040]
3.2.3、在搜索得到关键任务整体对应的最优工作位置后，对每一排的任务的逆运动学解进行验证，并对碰撞与机器人配置进行判断；如果机器人的配置和碰撞条件都符合要求，则该排任务对应该最优工作位置，如果不符合要求或搜索不到最优工作位置点，即关键任务整体对应的逆运动学解不存在时，对关键任务整体中的第一排关键任务单独进行逆运动学求解，并在下一排关键任务求解时重新回到3.2.1。
[0041]
进一步地，所述步骤4的具体过程包括以下步骤：
[0042]
4.1、根据蚁群算法，求解各工作位置点的遍历顺序；
[0043]
4.2、根据蚁群算法，求解相邻两个遍历点之间的路径；
[0044]
4.3、根据机器人到达工作位置点的位置和姿态，求解各个任务的遍历初始位置和遍历顺序。
[0045]
进一步地，所述的蚁群算法如下：
[0046]
假设有m只蚂蚁需要到达n个工作位置，用禁忌表tabuk表示蚂蚁k已经访问过的工作位置点，当tabuk已经包含所有工作位置时，蚂蚁k的一次迭代完成；
[0047]
当前工作位置到下一工作位置的启发式函数为：
[0048][0049]
η
ij
(t)＝1/d(i,j,turn)
[0050][0051]
其中，allowedk表示蚂蚁k下一步可以访问的工位，τ
ij
(t)表示时刻t路径(i,j)上的信息素量，η
ij
(t)表示路径(i,j)的启发式函数，α表示信息启发式因子，β表示期望启发式因子；d(i,j,turn)表示工位i到工位j的代价，其中stepmax表示机器人移动的最大步数，turn是转向次数，cw是移动代价，cr是转向代价；
[0052]
蚂蚁每次访问一个工作位置后会对所走过路径上遗留的信息素浓度更新，信息素浓度更新公式如下：
[0053][0054]
有益效果：
[0055]
本发明提供一种蒸汽发生器传热管检修机器人的任务规划方法，在不违反移动机器人的关节限制、不与水室边界碰撞的前提下，规划与任务对应的机器人位置和关节配置，可以有效的解决检查大量重复区域或不在任务范围内的传热管问题，使机器人的运动时间最短。
附图说明
[0056]
图1为任务规划的流程图；
[0057]
图2为工位点求解的流程图；
[0058]
图3为机器人的运动简图；
[0059]
图4为机器人本体和工具相关角度示意图；
[0060]
图5为蒸汽发生器检修机器人的末端工具工作空间；
[0061]
图6(a)和图6(b)为对应于机器人的两末端工具间距在机器人末端工具空间工作范围内的最优工作矩形区域、次优工作区域和沿着长度方向工作的区域；图6(a)对应最优工作矩形区域，图6(b)对应次优工作区域和沿着长度方向工作的区域；
[0062]
图7为区域处理的合并原则示意图；
[0063]
图8(a)和图8(b)为区域处理补全原则示意图；
[0064]
图9为三种工作方向示意图；
[0065]
图10为三个任务求解最优工位点的示意图；其中，黑色填充圆(位于图中心位置的
填充圆)为机器人的当前工位点位置，红色填充圆(五边形包围的填充圆)为求解出的工位交叉区域，五边形(三个半圆环形成的交集构成五边形)包围的红色填充圆为机器人的最优工位点；
[0066]
图11(a)至图11(e)分别为五组实验的机器人初始位置和给定的任务示意图；
[0067]
图12(a)至图12(h)为一组任务规划的结果；
[0068]
图13为不同管孔孔距下最优检查的方式。
具体实施方式
[0069]
针对于背景技术中的问题，因此，本发明基于我国sg传热管检修机器人任务规划的研究现状，设计一种检修传热管任务的规划算法，在合理划分检修任务的基础上，规划子区域对应的工作位置点位置并设计子区域遍历算法，使机器人完成全部的检修任务且运动时间短，总的完成任务数量与需要完成的任务数量的比值低。
[0070]
具体实施方式一：结合图1和图2说明本实施方式，
[0071]
以末端两个工具间隔2个管孔为例进行说明，所述的长度单位均以对应的管孔数量进行表示。机器人是一个三自由度的平面运动机器人，机器人的运动示意图如图3所示，机器人本体和工具相关角度示意图如图4所示，根据机器人的移动自由度和两个旋转自由度，可以求出两个工具所能到达的所有空间，即末端工具的工作空间，如图5所示。
[0072]
如果机器人在某个位置上能使末端工具扫查到工作空间中给定的任务管孔，则称该位置为该给定任务管孔对应的机器人工作位置点。在工具的工作空间内，存在无数个包含在其中的矩形，即为不同长度和宽度的矩形工作区域，将这种区域的宽度称为工具的工作宽度。由于工具间隔k个管孔，因此让工具最优检查的方式是让两个工具均能扫查，即矩形区域的宽度能保证工具以间隔k个管孔的方式沿着该宽度扫查完一列，则满足上述条件的工作宽度称为最优工作宽度，不同管孔孔距下最优检查的方式如图13所示。同理，当矩形区域的宽度保证工具间隔k个管孔沿着该宽度检查一列管孔时，大部分的管孔均能被两个工具同时检查，只多检查1个(对于整体而言是一列)不在矩形区域的管孔，满足这种条件的矩形宽度称为次优工作宽度。当矩形宽度为1时，工具只能沿着矩形的长度方向进行单排的扫查，这时工具的工作性能取决于矩形的长度，这种工作方式称为沿着长度方向的扫查，且工作宽度为1。
[0073]
按照步骤2中的公式取k＝2计算作业工具完成任务的矩形工作区域，如图6(a)和图6(b)所示，这些矩形的宽度即当前机器人的工具工作宽度。对于工具间隔2个管孔的机器人，工作宽度是4、8、12、16时能保证两个工具均能扫查，即这些矩形区域宽度能保证工具以间隔2个管孔的方式沿着该宽度扫查完一列，因此{{4},{8},{12},{16}}为k＝2时的最优工作宽度。而当矩形区域宽度为3时，工具间隔2个管孔沿着该宽度检查一列管孔时，大部分的管孔均能被2个工具同时检查，2个工具只多检查1个不在矩形区域的管孔，因此3为次优工作宽度。当矩形宽度为1时，工具只能沿着矩形的长度方向进行单排的扫查，这时工具的工作性能取决于矩形的长度，这种工作方式称为沿着长度方向的扫查，且工作宽度为1。给定任意宽度，都可以拆解为最优工作宽度、次优工作宽度和沿长度方向宽度的组合，比如宽度7拆解为最优工作宽度4和次优工作宽度3，宽度10可以拆解为最优工作宽度8和两个沿着长度方向的宽度1。
[0074]
根据求得的工作宽度，在工具的工作空间内找到宽度为该工作宽度，长度为使该区域在工作空间内部且最长的矩形区域，则该矩形区域的长度称为工作长度。在工具的工具空间内部搜索满足相应宽度的区域，记录所有区域的长度和宽度，即完成了对机器人工作的最优、次优和长度工作区域的求解。将所求得的工作区域的宽度和长度对应的组合依次表示为工作矩阵，即每一类工作矩阵为一个nx2的矩阵，n为最优工作宽度、次优工作宽度或者沿长度方向工作宽度中的工作宽度数量；第一列表示工作宽度，第二列表示该工作宽度在工具工作空间内对应的最大工作长度。
[0075]
将k＝2时的最优工作宽度{{4},{6},{8},{12},{16}}、次优工作宽度3、沿长度方向工作宽度1按此方法写为矩阵的形式，可得该机器人最优、次优和长度工作矩阵如下：
[0076][0077]
其中，tpb、tps、tpl分别为机器人工作的最优、次优和长度工作矩阵；k为两个检查工具的距离；
[0078]
本发明存在的三种可选检查方式，分别是沿着最优或次优工作宽度的方向、沿着长度的方向，以及最优或次优工作宽度与长度组合的方向完成检查任务，如图9所示。
[0079]
沿着某一工作宽度(包括沿着长度方向的工作宽度1)检查的一列管孔数量，称为该工作宽度的单位数量，而某一工作宽度的单位数量与工作长度的乘积就是该矩形工作区域在该种扫查方式下对应的全部需要扫查的管孔数量。
[0080]
由于在核工业实际应用中，任务区域常是分散的且形状不为标准形状；因此，本实例中给定分散且形状不为标准形状的任务区域；因此在评价工作方向前，应先根据所提出的补全原则和合并原则，对给定的任务区域进行补全和合并处理；
[0081]
本发明存在的三种可选检查方式，分别是沿着最优或次优工作宽度的方向、沿着长度的方向以及最优或次优工作宽度与长度组合的方向完成检查任务；定义沿着区域宽度w方向进行检查的单位任务数量为：
[0082][0083][0084]
其中，为向下取整函数；w
2k
对应的区域沿着最优宽度工作，w
remain
对应的区域沿着长度方向工作，w
k 1-w
2k-1
对应的区域则是沿着次优宽度方向工作；
[0085]
利用上述公式，代入相关数据，则该机器人工作宽度对应的检查任务的单位数量的表达式为：
[0086][0087]
机器人对应的检查任务区域宽度为w、区域长度为l的工作区域所需检查任务的总数量就是num(w)
×
l；
[0088]
为评价机器人沿某一方向作业的优越性，本发明提出评价函数评价某一作业方向的优越性，函数值越大，在某一方向作业的优越性越高。
[0089]
定义沿着宽度w方向进行检查的评价函数为：
[0090][0091]
其中，α
2k
为沿宽度方向的工作因子，α
2k
＞0，α
remain
为沿长度方向的工作因子，α
remain
＜0，α
2k-1
,α
2k-2
,
…
α
k 1
为沿着宽度和长度组合方向的工作因子，所有的工作因子均在[0,1]之间取值，利用工作因子为权重即可计算评价函数的值；
[0092]
利用上述公式计算工具检查任务评价函数的表达式为：
[0093][0094]
由于在核工业实际应用中，任务区域常是分散的且形状不为标准形状。因此，本实例中给定分散且形状不为标准形状的任务区域。因此在评价工作方向前，应先根据所提出的补全原则和合并原则，对给定的任务区域进行补全和合并处理。
[0095]
在补全原则示例中，对非长方形区域进行补全，补全为长方形区域，如图8(a)和图8(b)所示。该区域中加粗部分为任务区域，但该任务区域为不规则区域，因此利用矩形框对任务区域进行补全，且该矩形为补全该区域的最小的矩形。
[0096]
如图7所示，合并原则适用于一些距离较近、且同一方向的尺寸相同的p个任务区域，可以用一个宽/长为a的矩形区域覆盖p个宽/长为a1,a2,
…
,ap的矩形任务区域，且a＝a1＝a2＝
…
＝ap。合并后对合并区域进行判断，如果满足合并区域的评价函数值大于等于每个任务区域评价函数值的和，则保留该合并区域，否则保留原区域。
[0097]
合并原则能合并分散的较小区域，增强机器人作业时某方向搜索时的整体性。而补全原则是指对于一些不规则形状的区域，可以计算一个矩形区域，保证能覆盖所有任务，且该补全所得的矩形尺寸最小。该原则能进一步增强作业时规划的整体性。对任务区域进行合并和补全保证在分配任务的时候整体考虑，而不是逐个规划区域，有利于优化后续的工作位置点搜索的优化，能提高机器人作业的性能。
[0098]
在合并原则示例中，共有三个任务区域。工作因子根据实际情况确定，在本实例中取α4＝0.3，α3＝0.2，α
remain
＝-0.5。任务区域有一个共同尺寸(6)，合并后，经比较计算，区域的宽度为16，评价函数值为value(16)＝0.3，合并前的三个任务区域的宽度分别为6、4、3，因此评价函数值分别为value(6)＝-0.1，value(3)＝-0.15，value(4)＝0.3，计算value(16)＞value(6) value(3) value(4)，满足区域合并的要求，即合并后作业方向的优越性大于原作业方向的优越性之和，故可以合并区域。
[0099]
完成区域的补全和合并之后，对所有任务区域求解最优工作方向，即计算对应工作宽度的评价函数，函数值较大者即作为主要工作宽度。对长为10，宽为4的工作区域，工作方向为长度10的单位任务数量为num(10＝12，评价函数值为value(10)＝0.27，而长度4的单位任务数量为num(4＝4，评价函数值为工作方向为value(4)＝0.3，因此，定义宽度为4的方向是主要工作方向，根据主要工作方向确定最优工作方向。
[0100]
对工作位置求解时首先确定对机器人工作位置的搜索方向(搜索方向与最优工作方向垂直)。
[0101]
确定搜索方向：工作位置搜索有三种方向，以矩形区域沿着宽度方向的两边界距离机器人当前工作位置的代价作为基本判断标准，工作位置搜索的三种方向分别为从远到近、从近到远和从中间向两侧；从中间向两侧的搜索会破坏搜索长度方向上的整体性，从近到远的搜索会导致每次搜索只考虑距离较近处的任务，对较远处的任务没有提前的规划，每次求解出的最优工作位置都需要机器人小步挪动，增加运动时间；因此本发明选取从远到近的搜索方式；
[0102]
选取从远到近的搜索方式，搜索过程分为三步：
[0103]
第一步、判断整体搜索的长度：
[0104]
由于工作长度指给定工作宽度时使工作区域在工作空间内部且最长的矩形区域长度，因此本发明根据上述列出的最优、次优和长度对应工作矩阵，查找待搜索区域中某个子区域工作宽度的最大工作长度，将所有未搜索子区域的工作长度与工具工作空间内对应的最大工作长度对比，选取其中较小的进行整体搜索。
[0105]
整体搜索指对每个工作区域内的所有子区域进行搜索。传统方法对每个任务都进行计算，如图10所示，先计算每个任务的工作位置区域，再取所有区域的交点。而蒸发器的检修任务数量一般较大，故如果针对每个任务都进行搜索和计算，并寻找每个任务区域的交叉位置，计算量非常庞大。因此，可以利用整体搜索减少搜索和计算量。
[0106]
第二步、选取整体搜索的关键任务：
[0107]
在搜索时，通常将整个子区域的所有任务取到搜索列表，遍历寻找工作位置，这种搜索需要大量的计算。但实际上，一个子区域的工作位置往往只取决于某些特定任务管孔对应的工作位置，只搜索这些任务也能得到该子区域的最优工作位置，与搜索整个区域结果一致。因此，只对这些特定的任务管孔进行搜索，能减少搜索次数，减少计算量。
[0108]
这些特定的任务管孔可称为关键任务，通过搜索这些关键任务即可得到待搜索子区域的关键位置。一个子区域的关键任务通常包括子区域两侧的任务，但若只包括两侧的任务，由于所对应的工作位置有很多，导致选取的最优工作位置可能是错误的，因此，必须同时选择子区域中心的任务作为关键任务，保证搜索得到的工作位置区域包含的都是正确的工作位置。
[0109]
为减少判断任务的次数，在任务子区域中选取关键任务代替判断子区域内的所有任务。选择一个子区域的的两侧和中心都作为关键区域被判断，则此时工作位置的区域能包含所有可能的正确解，可从中选出正确的最优工作位置。例如对于长度为l的区域，选取关键任务为第[0]，[l/2]，[l]长度位置所对应的一排的全部任务。
[0110]
第三步、重新判断各排任务：
[0111]
在搜索得到关键任务整体(本实施方式中，第一次执行时是三排关键任务，后续可
能是出去第一排关键任务后剩余的关键任务整体)对应的最优工作位置后，对每一排的任务的逆运动学(ik)解进行验证，并对碰撞与机器人配置进行判断。其中进行逆运动学计算是根据任务计算工作位置的位置，碰撞判断是指对某个工作位置判断机器人机身和工具是否会与蒸发器外壳等碰撞。而是否满足机器人配置条件指在沿着宽度扫查时，是否满足每一排任务都能在求得的位置上扫查到。如果机器人的配置和碰撞条件都符合要求，则该排任务对应该最优工作位置，如果不符合要求或搜索不到最优工作位置点，即关键任务整体对应的逆运动学解不存在时，对关键任务整体中的第一排关键任务单独进行逆运动学求解，并在下一排关键任务求解时重新回到第一步。
[0112]
在得到了所有的子区域任务及其对应的最优工作位置后，对所有的工作位置进行路径规划。路径规划可以分为两步，第一步求出所有工作位置的到达顺序，第二步求出相邻两工作位置的最优路径。
[0113]
采用改进的蚁群算法进行工作位置顺序求解：
[0114]
首先，建立蚁群算法的数学模型：假设有m只蚂蚁需要到达n个工作位置，用禁忌表tabuk表示蚂蚁k已经访问过的工作位置点，当tabuk已经包含所有工作位置时，蚂蚁k的一次迭代完成。在基本蚁群算法中，需要定义当前工作位置到下一工作位置的启发式函数，这里选取在我们过去的工作中提出的代价函数作为距离的判断标准：
[0115][0116]
η
ij
(t)＝1/d(i,j,turn)
[0117][0118]
其中，allowedk表示蚂蚁k下一步可以访问的工位，τ
ij
(t)表示时刻t路径(i,j)上的信息素量，η
ij
(t)表示路径(i,j)的启发式函数，α表示信息启发式因子，β表示期望启发式因子。d(i,j,turn)表示工位i到工位j的代价，其中stepmax表示机器人移动的最大步数，turn是转向次数，cw是移动代价，cr是转向代价。
[0119]
蚂蚁每次访问一个工作位置后会对所走过路径上遗留的信息素浓度更新，信息素浓度更新公式如下：
[0120][0121]
在通过蚁群算法求得工作位置的最优访问顺序后，每个工作位置往往都对应多个检查任务，因此需要规划检查任务的作业顺序。
[0122]
对于子区域内的任务，选取往返式遍历方法，在选取距离机器人到达工作位置点后作业工具最近的点作起点后，沿着上文规划的长度方向，依次进行遍历，到达子区域边界后调转90
°
继续遍历，往复直至遍历完所有的子区域内的所有任务。
[0123]
在110
×
54的半圆形管板上对移动机器人进行任务规划实验，给定五组不同的任务，用按行搜索的算法m1、按列搜索的算法m2和本发明提出的算法m3进行任务规划和机器人的行走实验。五组实验的机器人初始位置和给定的任务如图11(a)至图11(e)所示。
[0124]
在该实验中，以图11(e)的初始位置和给定的任务进行检查说明，如图12(a)-图12(h)所示，机器人分别以三个工作宽度8、4、3进行检查，图12(a)和图12(b)为以工作宽度8进行工作的示意图，图12(c)和图12(d)为补全区域部分以工作宽度8进行工作的示意图，图12(e)和图12(f)为以工作宽度4进行工作的示意图，图12(g)和图12(h)为以工作宽度3进行工作的示意图；图中将任务区域分为三个子区域，并显示了每个子区域对应的开始任务和终止任务的机器人关节配置。
[0125]
用按行搜索的算法m1、按列搜索的算法m2和本发明提出的算法m3进行任务规划和机器人的行走实验以检查点数量、规划的工作位置点数量、所有工作位置的路径代价、计算时间和机器人的运行时间作为对比的主要指标，用蚁群算法计算路径时，蚂蚁数量为100，迭代次数150次。在记录运动时间时，只记录机器人将末端工具送到指定管孔的时间，不考虑工具进行后续检查作业的时间，具体如表1所示：
[0126]
表1
[0127][0128]
本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。