基于运动约束改进蚁群算法的移动机器人路径规划方法

：
1.本发明涉及移动机器人路径规划领域，具体为基于运动约束改进蚁群算法的移动机器人平滑路径规划方法。


背景技术：

2.伴随着人工智能技术的飞速发展，以无人机和无人车为代表的移动机终端在众多领域取得了优异的应用成果，无论是人们的生产生活、文体娱乐甚至军事领域，都越来越多地出现了这类无人系统的身影。就无人系统的发展来看，移动机器人在实际应用中还面临许多问题。其中，路径规划是移动机器人自主导航中非常重要的技术之一，其目的是让移动机器人在有障碍物的工作环境中，从初始点准确且快速地寻找一条能够避开所有障碍物到达目标点的最优路径。路径规划常用的方法可以分为传统规划方法和智能规划方法两大类。
3.传统的路径规划算法有a-star算法，快速随机树法(rapid_exploration random tree,rrt)和人工势场法。采用a-star算法进行移动机器人路径规划时规划出的路径转折点较多、路径不平滑且搜索时间长；采用人工势场法进行路径规划时，容易出现目标点不可达和陷入局部最优解。相比于传统的路径规划算法，智能规划算法更适合解决大规模下的路径规划问题。智能规划算法有蚁群算法，遗传算法和粒子群算法等。其中，蚁群算法具有较强的鲁棒性和自适应能力，在解决路径规划问题时效果良好。
4.蚁群算法是由dorigo等学者提出的一种模拟蚂蚁觅食行为的分布式智能仿生算法，可以实现从起始节点到目标节点的路径规划。蚁群算法具有正反馈高、鲁棒性强、分布式计算等优点，但也存在前期收敛速度慢，后期搜索停滞和易陷入局部最优解等问题。并且传统蚁群算法在规划路径时没有考虑移动机器人的运动学约束，规划的路径存在转弯次数过多，转弯角度过大等问题。


技术实现要素：

5.本发明的目的是提供基于运动约束改进蚁群算法的移动机器人路径规划方法，这种基于运动约束改进蚁群算法的移动机器人路径规划方法用于解决传统蚁群算法在规划路径时没有考虑移动机器人的运动学约束，规划的路径存在转弯次数过多，转弯角度过大的问题。
6.本发明解决其技术问题所采用的技术方案是：这种基于运动约束改进蚁群算法的移动机器人路径规划方法包括如下步骤：
7.步骤一：对移动机器人的工作环境地图进行栅格化建模，确定起始点和目标点，并初始化蚁群算法参数；
8.步骤二：初始化信息素矩阵，初始化禁忌表，并且根据禁忌表和障碍物信息构建可选节点；
9.步骤三：根据状态转移函数构建候选解，按照轮盘赌原则选择下一节点；其中，改
进状态转移函数的启发函数，在启发函数中考虑待选节点与目标点的距离并且融合转角惩罚因子，提高算法的启发性；
10.步骤四：根据下一节点，更新禁忌表并记录蚂蚁的路径节点和路径长度；
11.步骤五：引入多目标性能指标，将路径规划问题转化为多目标优化问题，将路径长度和转弯能耗2个评价指标作为多目标优化函数的约束条件；
12.步骤六：当蚂蚁数量达到预设的最大蚂蚁数量，本次迭代结束，记录本次迭代的最优路径和最差路径；通过双层信息素更新方式进行信息素更新，局部信息素更新采用奖励惩罚机制，全局信息素更新采取额外增加全局最优路径的信息素浓度，同时限制最大最小信息素的值；
13.步骤七：根据预设的最大迭代次数进行迭代，获得最优路径；
14.步骤八：采用改进的b样条曲线平滑方式对最优路径进行平滑处理，得到拟合程度高且满足移动机器人最小转弯半径约束的最优路径。
15.上述方案步骤一中建模过程是将工作环境地图抽象为x
×
y的栅格地图，每个栅格的大小为a
×
a，每个栅格有唯一的栅格编号i，编号i与栅格坐标的对应关系为其中(xi，yi)为栅格i的坐标，x为每一行网格数目，y为每一列网格数目，mod为求余运算，ceil为向上取整，通过矩阵来生成栅格图，0表示可以通过，1表示有障碍物。
16.上述方案步骤三中改进状态转移函数的启发函数为：
[0017][0018]
式中，η
ij
(t)表示的是节点i，j之间的启发函数，d
jg
为待选点到目标点欧式距离，f
cost
为前点到待选节点的转向惩罚代价；λ1和λ2是相应的权重系数，λ1+λ2＝1，λ1和λz的取值范围为[0，1]；
[0019]
转向惩罚代价因子f
cost
的大小与转角大小δθ正相关，转向角度越大，代价越大；转向惩罚代价因子f
cost
为：
[0020]
δθ＝|θ
i-θj|
[0021][0022]
式中，p为惩罚因子，取值范围在[2，4]最优，θi移动机器人在栅格i的偏航角，θj移动机器人在栅格j的偏航角。
[0023]
上述方案步骤五中多目标优化函数表达式如下：
[0024][0025][0026][0027]
s(p)＝k
l
×
l(p)+ke×
e(p)
[0028]kl
+ke＝1
[0029]
式中，p为总的路径，n为路径经过的网格数，pi为路径上的第i个网格，(xi，yi)为第i个网格的坐标值，d(pi，p
i+1
)为pi到p
i+1
的距离，p为转角惩罚系数，l(p)为路径所有节点的欧式距离之和；e(p)表示路径转弯总能耗；s(p)为综合质量，是路径长度和路径转弯能耗分别以不同比重组成的综合指标。
[0030]
上述方案步骤六中双层信息素更新方式的计算公式如下：
[0031][0032][0033][0034][0035]
式中，k表示第k只蚂蚁，m表示总的蚂蚁数，t表示的是当前迭代次数，δτ
ij
(t)为蚂蚁在第t次迭代过程中在节点i，j的信息素增量，ρ表示的是信息素挥发因子，q表示的是信息素强度，sk(t)为路径的综合性能指标，由路径长度lk(t)和转弯总能耗tk(t)组成，sk(t)越小综合性能越好；s
best
为本次迭代的最优路径的综合质量，s
worse
为本次迭代的最差路径的综合质量，s
best
为全局最优路径的综合质量；w为信息素奖惩因子，当路径的综合性能比本次迭代的平均综合性能高时，w为正，信息素浓度增加，反之，为负，信息素浓度降低；是增加全局最优路径的信息素浓度；
[0036]
将信息素的取值范围限定在τ
min
到τ
max
，表达式如下：
[0037][0038]
上述方案中步骤八包括以下几个步骤：
[0039]
1)控制点的提取：
[0040]
采取增加控制点的策略，平滑路径时，根据移动机器人转角的大小，采取不同的系数增加控制点，当转角大于90
°
，在距拐点1/3处插入控制点；当转角小于90
°
，在距拐点1/5处插入控制点；
[0041]
2)最小转弯半径的拟合：
[0042]
平滑路径满足最大曲率约束，对拐点处的曲率进行计算，当拐点处的路径曲率不符合曲率的约束条件，采取一种基于中心点的平滑方式对该路段重新平滑，用两个新增的节点将不符合曲率约束的节点替换掉。
[0043]
上述基于运动约束改进蚁群算法的移动机器人路径规划方法在基于运动约束改进蚁群算法的移动机器人平滑路径规划系统中进行，所述基于运动约束改进蚁群算法的移动机器人平滑路径规划系统包括：
[0044]
地图栅格化模块，其用于获取移动机器人的工作环境，并对工作环境进行栅格化
处理，得到栅格地图，确定移动机器人的起始节点，目标节点和障碍物节点的信息；
[0045]
路径搜索模块，其用于计算启发函数信息矩阵和状态转移概率矩阵，并根据计算结果通过轮盘赌算法选择下一个节点，当抵达目标点或者陷入死锁节点，结束路径搜索；
[0046]
路径保存模块，其用于记录蚂蚁的路径节点和路径长度；
[0047]
信息素更新模块，当所有蚂蚁完成一次迭代后，根据信息素更新规则更新信息素浓度，直至达到最大迭代次数，得到最终的最优路径；
[0048]
路径平滑模块，采用改进的b样条平滑方法对最优路径进行平滑处理，得到拟合程度高且满足移动机器人最小转弯半径约束的最优路径。
[0049]
有益效果：
[0050]
1.本发明针对传统蚁群算法的启发函数全局搜索性能差的缺点，引入距离函数和转角惩罚函数作为启发因子，提高算法的全局搜索性。
[0051]
2、本发明引入多目标性能指标，将路径规划问题转化为多目标优化问题，以路径长度和转弯能耗2个评价指标作为多目标函数的约束条件，提高路径搜索的质量。
[0052]
3、本发明改进信息素的更新方式，提出一种新型的双层信息素更新方式。局部信息素更新采用奖励惩罚机制，提高算法的收敛速度。全局信息素更新采取额外增加全局最优路径的信息素浓度，同时限制最大最小信息素的值，防止算法陷入局部最优解。
[0053]
4、本发明提出一种改进的b样条曲线平滑算法，使生成的路径满足移动机器人的运动学约束。
[0054]
5、本发明对传统蚁群算法和平滑路径算法进行了优化和改进，改进的蚁群算法加快了算法的收敛速度，避免局部最优解并且生成的最优路径转折点更少，转角更小。
[0055]
6、本发明通过改进的b样条平滑算法对最优路径的转折点进行平滑，对路径曲率不满足移动机器人运动学约束的转折点进行特殊处理，得到平滑的最优路径。
[0056]
7、本发明得到的路径距离更短，转折点更少和并且满足移动机器人最小转弯半径约束，使得机器人在工作环境种更能够灵活地行驶，提高了机器人的搜索效率。
附图说明
[0057]
图1基于运动约束改进蚁群算法的移动机器人平滑路径规划方法的流程图；
[0058]
图2移动机器人二维栅格环境地图示意图；
[0059]
图3移动机器人偏航角示意图；
[0060]
图4阿克曼底盘无人车的运动学模型图；
[0061]
图5不同转角下的增加b样条曲线控制点的方案图；
[0062]
图6改进的b样条曲线平滑方式操作示意图；
[0063]
图710
×
10的栅格地图路径平滑算法对比实验结果图；
[0064]
图8分散型障碍物的复杂环境示意图；
[0065]
图9分散型障碍物环境下的传统蚁群算法路径规划图；
[0066]
图10分散型障碍物环境下的改进蚁群算法路径规划图；
[0067]
图11分散型障碍物环境下的算法收敛比较图；
[0068]
图12基于运动约束改进蚁群算法的移动机器人平滑路径规划系统结构示意图。
具体实施方式：
[0069]
下面结合附图对本发明做进一步说明：
[0070]
实施例1：
[0071]
这种基于运动约束改进蚁群算法的移动机器人平滑路径规划方法，本实例以阿克曼模型的自动导引车(agv)为例，流程图如图1所述，路径规划方法包括以下步骤：
[0072]
步骤1：采用栅格法对移动机器人的工作环境进行栅格化建模，建模过程是将环境地图抽象为x
×
y的栅格地图，对于形状不规则的障碍物，如果分割后的大小没有完全占据一个栅格，为了路径规划方便，视作一个完整的障碍栅格。每个栅格的大小为a
×
a，每个栅格有唯一的栅格编号i，编号i与栅格坐标的对应关系为其中(xi，yi)为栅格i的坐标，x为每一行网格数目，y为每一列网格数目，mod为求余运算，ceil为向上取整。如图2为移动机器人二维栅格环境地图示意图，其中，白色栅格表示可行栅格，黑色栅格表示障碍栅格。
[0073]
步骤2：初始化蚁群算法的参数，初始化信息素矩阵，初始化禁忌表，并且根据禁忌表和障碍物信息构建可选节点。其中，设置算法的最大迭代次数k，蚂蚁的数目m，信息素启发因子α，信息素蒸发系数p，期望启发因子β。
[0074]
步骤3：根据状态转移函数构建候选解，按照轮盘赌原则选择下一节点；状态转移函数如下所示：
[0075][0076]
式中，τ
ij
(t)表示的是节点i，j之间的信息素浓度，η
ij
(t)表示的是节点i，j之间的启发函数。allowedk表示的是节点候选解。
[0077]
改进状态转移函数的启发函数，在启发函数中考虑待选节点与目标点的距离并且融合转角惩罚因子，提高算法的启发性。
[0078]
改进的启发函数体公式为：
[0079][0080]
公式中，d
jg
为待选点到目标点欧式距离，f
cost
表示的是当前点到待选节点的转向惩罚代价。η
ij
(t)表示的是节点i，j之间的启发函数，d
jg
为待选点到目标点欧式距离，f
cost
表示的是当前点到待选节点的转向惩罚代价。λ1和λ2是相应的权重系数，λ1+λ2＝1，λ1和λ2的取值范围为[0，1]。
[0081]
无人车在经过障碍物群时，若只是把路径最短作为路径规划的参考因素，则生成的最优路径会存在拐点过多，拐角过大的问题。无人车则会因为转弯次数过多，实际运行时间和能耗大大增加，规划结果并不理想。因此，本文将转向惩罚代价因子加入到启发函数中，使得蚂蚁在节点扩展时尽可能不改变运动方向沿直线运动，规划出转弯次数更少，转弯角度更小的综合最优路径。
[0082]
转向惩罚代价因子f
cost
的大小与转角大小δθ正相关，转向角度越大，代价也越
大。这样设计时为了让移动机器人在节点扩展的时候，尽可能选择转向角度小的节点。转向惩罚代价因子f
cost
具体表达式为：
[0083]
δθ＝|θ
i-θj|
[0084][0085]
公式中，p为惩罚因子，取值范围在[2，4]最优。θi移动机器人在栅格i的偏航角，θj移动机器人在栅格j的偏航角。移动机器人的偏航角示意图如图3所示，偏航角大小的计算公式：
[0086][0087][0088]
公式中，(xi，yi)表示的是移动机器人在栅格i的坐标，(x
i+1
，y
i+1
)是栅格j的坐标。
[0089]
步骤4：更新禁忌表并记录蚂蚁的路径节点和路径长度；判断蚂蚁是否陷入死锁，如果候选解为空，则蚂蚁陷入死锁，结束本次循环。
[0090]
步骤5：引入多目标性能指标，将路径规划问题转化为多目标优化问题，将路径长度和转弯能耗2个评价指标作为多目标优化函数的约束条件。
[0091]
步骤6：判断蚂蚁是否达到目标点并判断是否达到预设的最大蚂蚁数量，若是，则按照改进的信息素更新方式进行信息素的更新。
[0092]
其中信息素的更新方式的改进是多目标优化指标和双层信息素更新方式。
[0093]
以路径长度和转弯能耗2个评价指标作为多目标函数的约束条件，优化路径规划；多目标优化函数表达式如下：
[0094][0095][0096][0097]
s(p)＝k
l
×
l(p)+ke×
e(p)
[0098]kl
+ke＝1
[0099]
公式中，p表示的是总的路径，n是路径经过的网格数，pi表示路径上的第i个网格，(xi，yi)表示的是第i个网格的坐标值，d(pi，p
i+1
)表示的是pi到p
i+1
的距离，p表示转角惩罚系数。l(p)为路径所有节点的欧式距离之和；e(p)表示路径转弯总能耗；s(p)为综合质量，是路径长度和路径转弯能耗分别以不同比重组成的综合指标。
[0100]
在此基础上，改进的双层信息素更新方式公式如下：
[0101][0102]
[0103][0104][0105]
公式中，k表示第k只蚂蚁，m表示总的蚂蚁数，t表示的是当前迭代次数，δτ
ij
(t)为蚂蚁在第t次迭代过程中在节点i，j的信息素增量，ρ表示的是信息素挥发因子，q表示的是信息素强度，sk(t)为路径的综合性能指标，由路径长度lk(t)和转弯总能耗tk(t)组成。sk(t)越小综合性能越好。s
best
为本次迭代的最优路径的综合质量，s
worse
为本次迭代的最差路径的综合质量，s
best
为全局最优路径的综合质量。w为信息素奖惩因子，当路径的综合性能比本次迭代的平均综合性能高时，w为正，信息素浓度增加，反之，为负，信息素浓度降低。是增加全局最优路径的信息素浓度。
[0106]
为了防止经过多次迭代后，某条路径上的信息素值可能远大于或者远小于其他路径，使得搜索不能继续进行下去，过早的收敛陷入局部最小值。本文借鉴最大最小蚂蚁系统，将信息素的取值范围限定在τ
min
到τ
max
，表达式如下：
[0107][0108]
步骤7：判断迭代次数是否达到预设的最大迭代次数，若是，则结束迭代，获得最优路径；若否，则继续迭代。
[0109]
步骤8：采用改进的b样条曲线平滑方式对最优路径进行平滑处理，得到拟合程度高且满足移动机器人最小转弯半径约束的最优路径。
[0110]
本实例选择阿克曼模型的自动导引车(agv)为例，其运动学模型如图4所示，机器人的位姿为(x，y，θ)，x和y是机器人的中心在坐标系的横、纵坐标，θ为机器人的偏航角，为无人车前轮的转向角，l为机器人底盘的轴距。由于机器人的特性，机器人在转向时存在最大转向角即
[0111][0112]
机器人的转弯半径与转向角的关系为
[0113][0114]
因为存在最大转向角，机器人运动的路径曲率存在上限，即
[0115][0116]
式中，r
min
为最小转弯半径。因此当移动机器人转向时，存在最小转弯半径约束，规划的最优路径应该满足最大曲率约束。
[0117]
b样条曲线可以分为均匀b样条曲线、非均匀b样条曲线、准均匀b样条曲线和分段贝塞尔曲线四种类型。其中，均匀b样条曲线和非均匀b样条曲线在拟合时不经过首尾端点，分段贝塞尔曲线内部控制点平滑不连续。为了使优化后的路径覆盖起点和目标点且保证路径平滑连续，通常采用三次准均匀b样条曲线对最优路径进行平滑处理。三次准均匀b样条曲线的多项式表达式如下：
[0118][0119]
式中，p为控制点坐标，u为矢量节点，n为控制点数量，k为b样条曲线的阶次，i为第i个节点矢量，控制点或基函数，b
i，k
(u)为第i个k阶b样条基函数，与控制点pi相对应。
[0120]
当n＝3时，则有三次b样条曲线的基函数为：
[0121][0122]
由表达式可知，b样条曲线由基函数和控制点决定。针对b样条曲线拟合程度低和曲率大的问题，本发明提出一种改进三次准均匀b样条曲线的路径平滑算法，得到一条满足机器人最大曲率约束且拟合度高的平滑路径。改进的b样条路径平滑算法包括以下几个步骤：
[0123]
1)控制点的提取
[0124]
蚁群算法规划出来的路径通常是一条折线，在拐点的曲率变化大且不连续，提取拐点作为b样条曲线的控制点。为了让平滑后的曲线更逼近最优路径，采取增加控制点的策略。通常来讲，移动机器人的转角为钝角时路径的曲率远大于转角为锐角时路径的曲率。所以，平滑路径时，锐角处的路径应该比钝角处的路径具有更高的拟合程度。本文根据移动机器人转角的大小，采取不同的系数增加控制点。当转角大于90
°
，在距拐点1/3处插入控制点。当转角小于90
°
，在距拐点1/5处插入控制点。不同转角下增加b样条曲线控制点的方案如图5所示。
[0125]
2)最小转弯半径的拟合
[0126]
根据移动机器人的运动学模型，移动机器人在运动过程中，存在最小转弯半径，所以平滑路径应该满足最大曲率约束。三次准均匀的b样条曲线的曲率计算公式如下所示:
[0127][0128]
转折点的路径曲率应该满足ρ(u)≤ρ
max
。所以，改进的平滑算法主要内容为：对拐点处的曲率进行计算，当拐点处的路径曲率不符合曲率的约束条件，采取一种基于中心点的平滑方式对该路段重新平滑。该方法就是用两个新增的节点将不符合曲率约束的节点替换掉。操作示意图如图6所示，以距离拐点半个栅格大小的距离做等腰三角形，得到两个新的控制点。将原来的钝角转角化成两个锐角转角。使再次拟合得到的路径满足最大曲率要求。
[0129]
本发明通过一种基于运动约束改进蚁群算法的移动机器人平滑路径规划系统进行，如图12所示，这种基于运动约束改进蚁群算法的移动机器人平滑路径规划系统包括：
[0130]
地图栅格化模块，其用于获取移动机器人的工作环境，并对环境进行栅格化处理，得到栅格地图，确定移动机器人的起始节点，目标节点和障碍物节点的信息。
[0131]
路径搜索模块，其用于计算启发函数信息矩阵和状态转移概率矩阵，并根据计算结果通过轮盘赌算法选择下一个节点。当抵达目标点或者陷入死锁节点，结束路径搜索。
[0132]
路径保存模块，记录蚂蚁的路径节点和路径长度。
[0133]
信息素更新模块，当所有蚂蚁完成一次迭代后，根据信息素更新规则更新信息素浓度，直至达到最大迭代次数，得到最终的最优路径；
[0134]
路径平滑模块，采用改进的b样条平滑方法对最优路径进行平滑处理，得到拟合程度高且满足移动机器人最小转弯半径约束的最优路径。
[0135]
本发明所述的基于运动约束改进蚁群算法的移动机器人平滑路径规划方法中的步骤存储于计算机可读存储介质。
[0136]
这种基于运动约束改进蚁群算法的移动机器人平滑路径规划系统还包括电子设备，电子设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述所述的基于运动约束改进蚁群算法的移动机器人平滑路径规划方法中的步骤。
[0137]
本发明的实验验证：
[0138]
为了验证改进的b样条曲线路径平滑算法的有效性，在10x10的栅格地图中进行验证实验。实验结果如图7所述。由图7可以看出，改进的b样条曲线平滑算法得到的路径比传统的b样条曲线平滑路径拟合程度更高，且满足最大曲率约束。
[0139]
为了验证本发明提供方法的可行性，在图8所示的分散性障碍物复杂环境中，将上述的基于运动约束改进蚁群算法的移动机器人平滑路径规划方法进行模拟仿真，与传统的蚁群算法进行比较。结合图9-图11，可以看出传统蚁群算法容易陷入局部最优解，收敛速度慢，规划出来的路径拐角多，不够平滑。本文提出的改进蚁群算法的最优路径长度较传统蚁群算法缩短了54.54％；搜索效率(迭代次数)较基本蚁群算法提高了49.2％；拐点数目较传统蚁群算法减少了82.5％。综上所述，在分散性障碍物复杂环境的栅格地图中，本文提出的改进蚁群算法在路径长度，收敛速度(用迭代次数表示)和平滑度(用转弯次数表示)的表现均优于aco。
[0140]
传统蚁群算法和本发明的改进方法在分散性障碍物环境的路径结果如表1所示。
[0141]
表1分散性障碍物环境下两种算法结果对比
[0142] 传统蚁群算法本发明算法最优路径长度68.6643.94迭代次数4420拐点数目407
[0143]
本发明提供了一种基于运动约束改进蚁群算法的移动机器人平滑路径规划方法及系统，对传统蚁群算法的启发函数，最优路径的评判指标和信息素的更新规则进行改进，得到一条综合考虑路径长度和转弯次数的最优路径。并且考虑移动机器人在转弯过程中存在最小转弯半径约束，采用改进的b样条曲线对最优路径的转折点进行平滑处理，得到平滑的最优路径。
[0144]
本发明改进启发函数，在启发函数中考虑待选节点与目标点的距离并且融合转角惩罚因子，计算启发函数信息素矩阵；计算状态转移矩阵，由轮盘赌原则选择下一个节点；以路径长度和转弯能耗2个评价指标作为多目标函数的约束条件，将路径规划问题转化为
多目标优化问题；判断蚂蚁是否达到预设的最大蚂蚁数量，若是，根据本发明提出的双层信息素更新方式更新信息素；若迭代次数达到预设的最大迭代次数，则输出最优轨迹；采用改进的b样条曲线平滑方式对最优路径进行平滑处理，得到拟合程度高且满足移动机器人最小转弯半径约束的最优路径。本发明得到的路径距离更短，转折点更少和并且满足移动机器人最小转弯半径约束，使得机器人在工作环境种更能够灵活地行驶，提高了机器人的搜索效率。

技术特征：
1.一种基于运动约束改进蚁群算法的移动机器人路径规划方法，其特征在于包括以下步骤：步骤一：对移动机器人的工作环境地图进行栅格化建模，确定起始点和目标点，并初始化蚁群算法参数；步骤二：初始化信息素矩阵，初始化禁忌表，并且根据禁忌表和障碍物信息构建可选节点；步骤三：根据状态转移函数构建候选解，按照轮盘赌原则选择下一节点；其中，改进状态转移函数的启发函数，在启发函数中考虑待选节点与目标点的距离并且融合转角惩罚因子，提高算法的启发性；步骤四：根据下一节点，更新禁忌表并记录蚂蚁的路径节点和路径长度；步骤五：引入多目标性能指标，将路径规划问题转化为多目标优化问题，将路径长度和转弯能耗2个评价指标作为多目标优化函数的约束条件；步骤六：当蚂蚁数量达到预设的最大蚂蚁数量，本次迭代结束，记录本次迭代的最优路径和最差路径；通过双层信息素更新方式进行信息素更新，局部信息素更新采用奖励惩罚机制，全局信息素更新采取额外增加全局最优路径的信息素浓度，同时限制最大最小信息素的值；步骤七：根据预设的最大迭代次数进行迭代，获得最优路径；步骤八：采用改进的b样条曲线平滑方式对最优路径进行平滑处理，得到拟合程度高且满足移动机器人最小转弯半径约束的最优路径。2.根据权利要求1所述的基于运动约束改进蚁群算法的移动机器人路径规划方法，其特征在于：所述步骤一中建模过程是将工作环境地图抽象为x
×
y的栅格地图，每个栅格的大小为a
×
a，每个栅格有唯一的栅格编号i，编号i与栅格坐标的对应关系为其中(x
i
,y
i
)为栅格i的坐标，x为每一行网格数目，y为每一列网格数目，mod为求余运算，ceil为向上取整，通过矩阵来生成栅格图，0表示可以通过，1表示有障碍物。3.根据权利要求2所述的基于运动约束改进蚁群算法的移动机器人路径规划方法，其特征在于：所述步骤三中改进状态转移函数的启发函数为：式中，η
ij
(t)表示的是节点i，j之间的启发函数，d
ig
为待选点到目标点欧式距离，f
cost
为前点到待选节点的转向惩罚代价；λ1和λ2是相应的权重系数，λ1+λ2＝1，λ1和λ2的取值范围为[0，1]；转向惩罚代价因子f
cost
的大小与转角大小δθ正相关，转向角度越大，代价越大；转向惩罚代价因子f
cost
为：δθ＝|θ
i-θ
j
|
式中，p为惩罚因子，取值范围在[2，4]最优，θ
i
移动机器人在栅格i的偏航角，θ
j
移动机器人在栅格j的偏航角。4.根据权利要求3所述的基于运动约束改进蚁群算法的移动机器人路径规划方法，其特征在于：所述步骤五中多目标优化函数表达式如下：特征在于：所述步骤五中多目标优化函数表达式如下：特征在于：所述步骤五中多目标优化函数表达式如下：s(p)＝k
l
×
l(p)+k
e
×
e(p)k
l
+k
e
＝1式中，p为总的路径，n为路径经过的网格数，p
i
为路径上的第i个网格，(x
i
，y
i
)为第i个网格的坐标值，d(p
i
，p
i+1
)为p
i
到p
i+1
的距离，p为转角惩罚系数，l(p)为路径所有节点的欧式距离之和；e(p)表示路径转弯总能耗；s(p)为综合质量，是路径长度和路径转弯能耗分别以不同比重组成的综合指标。5.根据权利要求4所述的基于运动约束改进蚁群算法的移动机器人路径规划方法，其特征在于：所述步骤六中双层信息素更新方式的计算公式如下：特征在于：所述步骤六中双层信息素更新方式的计算公式如下：特征在于：所述步骤六中双层信息素更新方式的计算公式如下：特征在于：所述步骤六中双层信息素更新方式的计算公式如下：式中，k表示第k只蚂蚁，m表示总的蚂蚁数，t表示的是当前迭代次数，δτ
ij
(t)为蚂蚁在第t次迭代过程中在节点i，j的信息素增量，ρ表示的是信息素挥发因子，q表示的是信息素强度，s
k
(t)为路径的综合性能指标，由路径长度l
k
(t)和转弯总能耗t
k
(t)组成，s
k
(t)越小综合性能越好；s
best
为本次迭代的最优路径的综合质量，s
worse
为本次迭代的最差路径的综合质量，s
best
为全局最优路径的综合质量；w为信息素奖惩因子，当路径的综合性能比本次迭代的平均综合性能高时，w为正，信息素浓度增加，反之，为负，信息素浓度降低；是增加全局最优路径的信息素浓度；将信息素的取值范围限定在τ
min
到τ
max
，表达式如下：
6.根据权利要求5所述的基于运动约束改进蚁群算法的移动机器人路径规划方法，其特征在于：所述步骤八包括以下几个步骤：1)控制点的提取：采取增加控制点的策略，平滑路径时，根据移动机器人转角的大小，采取不同的系数增加控制点，当转角大于90
°
，在距拐点1/3处插入控制点；当转角小于90
°
，在距拐点1/5处插入控制点；2)最小转弯半径的拟合：平滑路径满足最大曲率约束，对拐点处的曲率进行计算，当拐点处的路径曲率不符合曲率的约束条件，采取一种基于中心点的平滑方式对该路段重新平滑，用两个新增的节点将不符合曲率约束的节点替换掉。7.根据权利要求6所述的基于运动约束改进蚁群算法的移动机器人路径规划方法，其特征在于：所述基于运动约束改进蚁群算法的移动机器人路径规划方法在基于运动约束改进蚁群算法的移动机器人平滑路径规划系统中进行，所述基于运动约束改进蚁群算法的移动机器人平滑路径规划系统包括：地图栅格化模块，其用于获取移动机器人的工作环境，并对工作环境进行栅格化处理，得到栅格地图，确定移动机器人的起始节点，目标节点和障碍物节点的信息；路径搜索模块，其用于计算启发函数信息矩阵和状态转移概率矩阵，并根据计算结果通过轮盘赌算法选择下一个节点，当抵达目标点或者陷入死锁节点，结束路径搜索；路径保存模块，其用于记录蚂蚁的路径节点和路径长度；信息素更新模块，当所有蚂蚁完成一次迭代后，根据信息素更新规则更新信息素浓度，直至达到最大迭代次数，得到最终的最优路径；路径平滑模块，采用改进的b样条平滑方法对最优路径进行平滑处理，得到拟合程度高且满足移动机器人最小转弯半径约束的最优路径。

技术总结
本发明涉及的是基于运动约束改进蚁群算法的移动机器人路径规划方法，它包括：对移动机器人的工作环境地图进行栅格化建模；在启发函数中考虑待选节点与目标点的距离并且融合转角惩罚因子，计算启发函数信息素矩阵；计算状态转移矩阵，由轮盘赌原则选择下一个节点；以路径长度和转弯能耗2个评价指标作为多目标函数的约束条件，将路径规划问题转化为多目标优化问题；蚂蚁数量达到预设的最大蚂蚁数量，根据双层信息素更新方式更新信息素；若迭代次数达到预设的最大迭代次数，输出最优轨迹；采用改进的B样条曲线平滑方式对最优路径进行平滑处理，得到满足移动机器人最小转弯半径约束的最优路径。本发明得到的路径距离更短，提高了机器人的搜索效率。了机器人的搜索效率。了机器人的搜索效率。


技术研发人员：霍凤财 董宏丽 朱帅 任伟建 康朝海 路敬祎 张会珍
受保护的技术使用者：东北石油大学
技术研发日：2023.06.13
技术公布日：2023/8/5