本发明涉及一种基于蚁群改进和尖峰平滑的移动机器人路径规划方法，属于机器人路径规划、人工智能等领域。
背景技术：
近年来，机器人在生活中的应用越来越广泛，移动机器人作为机器人应用中的重要分支，其在生产生活中的重要性也逐渐体现出来。移动机器人路径规划作为机器人自主导航的一个重要问题备受关注，机器人要完成某项任务，先决条件是要到达任务区域，所以移动机器人路径规划问题是机器人领域中的重要研究内容。
蚁群算法(Ant Colony Algorithn，ACA)又称为蚂蚁算法，由Marco Dorigo于1992年在他的博士论文中提出，其启发于蚂蚁群体可以通过信息素进行通信而体现出的智能行为。蚂蚁从蚁窝到食物的最优路径的选择是通过蚁群的群体智能行为体现的。蚂蚁在觅食的过程中，以信息素为媒介进行交流的，蚂蚁会在经过的路上留下信息素，并且能在觅食过程中感知该信息素的浓度，来指导自己的行为，信息素的浓度与路径的长度成反比，蚂蚁总是朝着信息素浓度高的方向移动，蚂蚁群体的觅食行为就是一种对信息素的正反馈现象，如果某一条路径越短，上面经过的蚂蚁就越多，信息素也会越多的遗留，也就是信息素的浓度越高，后面经过的蚂蚁选择这条路的概率就越大，经过这种正反馈过程，蚁群就会选择出最优路径。
国内外很多研究者对移动机器人路径规划做了很多研究，也出现了很多解决此类问题的方法，常见的有：遗传算法、神经网络、粒子群算法、A*算法等等。遗传算法的全局搜索能力比较强，但是其缺点是如果环境发生变化时需要重新建立相应的数学模型，使得运作成本增加，并且规划效率较低。用神经网络进行路径规划很难获取具有代表性的神经网络训练样本。A*算法由于其中的启发式函数很难做到完全正确，这样使得搜索过程会陷入“死循环”。粒子群算法的局部寻优能力较差，并且其速度和位置更新公式不够完善，使得路径规划的计算效率和可靠性受到影响。蚁群算法是一种典型的启发式搜索算法，具有较强的鲁棒性、优良的分布式计算、易于与其他算法结合等优点。基本蚁群算法有求解速度慢，容易陷入局部最优解的缺点；上述的多数算法都忽略了路径规划中对路径平滑性的要求，这样会使得机器人平衡性差，且在路径尖峰处会产生不必要的能量损耗。
技术实现要素：
针对以上现有技术中的不足，本发明提供了一种基于蚁群改进和尖峰平滑的移动机器人路径规划方法，融入了精英蚂蚁策略和基于中心点的平滑方法。
本发明的技术方案是：一种基于蚁群改进和尖峰平滑的移动机器人路径规划方法，所述方法步骤如下：
S1、利用栅格法对移动机器人运行空间进行环境建模；
S2、设置参数，对蚂蚁个数M、迭代次数N、蚂蚁起始点和终止点初始化，启发因子α、期望启发因子β、信息素挥发系数ρ、信息素增加强度系数Q、期望偏转角度θ1、禁忌表Bk初始化；
S3、M只蚂蚁从初始点出发；
S4、蚂蚁个体k根据状态转移概率公式以最大转移概率从栅格i转移到邻接栅格j；
S5、蚂蚁个体k每转移一次，将栅格j加入禁忌表Bk；
S6、根据精英策略信息素更新公式τij(t+1)更新信息素，用于状态转移概率公式里τij(t)的更新；其中，τij(t+1)表示t+1时刻蚂蚁个体k从栅格i点转移到栅格j点的信息素强度；
S7、判断所有的蚂蚁是否到达终点：若没有，则重复步骤S4-S6，直至所有的蚂蚁到达终止点；若有，则进行步骤S8；
S8、选出M条路径中距离最短的路径作为蚁群历经所有点后的最优路径并保存；
S9、判断是否达到最大迭代次数Nmax：若没有，则跳转到步骤S3；若达到，则从N条最优路径中选择距离最短的一条作为预规划路径，并执行步骤S10；
S10、引入基于中心点的平滑方法对预规划路径进行平滑操作并输出平滑路径。
所述步骤S2中，蚂蚁个数M为50，迭代次数N为200，启发因子α为1，期望启发因子β为7，信息素蒸发系数ρ为0.7，信息素增加强度系数Q为1，机器人的起始点坐标为(0.5,16.5)，终止点坐标为(16.5,0.5)，期望偏转角度θ1为155°。
所述步骤S10具体为：通过在预规划路径的拐点处增加新节点来实现的，用新的节点代替旧节点来完成路径的平滑处理，是否添加新的节点与拐角的角度期望值α1有关，若两条相邻直线的实际拐角θ2小于等于θ1，则在两条线段的可行域之间取线段的中点(x-new1,y-new1)和(x-new2,y-new2)，再判断新节点与两边构成的拐角角度是否满足大于角度期望值α1：若不满足，继续进行上述变换，寻找新的拐点(x-new1,y-new1)和(x-new2,y-new2)；满足角度条件后，删除旧拐点，以新拐点代替；以此判断路径内的其他拐点是否满足条件，不断重复，使得整条路径的拐角都大于角度期望值。
本发明的有益效果是：本发明融入了精英蚂蚁策略和基于中心点的平滑方法，引入精英策略，解决局部最优的缺点；基于中心点的平滑方法对路径尖峰有修正作用，使得路径拐角更为平滑，机器人实体能在拐弯的过程中平稳前进，同时减少在路径尖峰处不必要的能量损耗。该改进蚁群算法应用于移动机器人路径规划能得到一条长度较短相对平滑的曲线路径。
附图说明
图1为改进蚁群算法应用于移动机器人路径规划的流程图；
图2栅格环境图；
图3为中心点的平滑方法示意图；
图4为17×17栅格环境下传统蚁群算法路径规划图；
图5为17×17栅格环境下引入平滑方法后的改进ACA路径规划图；
图6为20×20栅格环境下传统蚁群算法路径规划图；
图7为20×20栅格环境下引入精英策略的ACA路径规划图；
图8为20×20栅格环境下引入基于中心点的平滑方法改进后的ACA路径规划图。
具体实施方式
实施例1：如图1-8所示，一种基于蚁群改进和尖峰平滑的移动机器人路径规划方法，所述方法步骤如下：
S1、利用栅格法对移动机器人运行空间进行环境建模；
S2、设置参数，对蚂蚁个数M、迭代次数N、蚂蚁起始点和终止点初始化，启发因子α、期望启发因子β、信息素挥发系数ρ、信息素增加强度系数Q、期望偏转角度θ1、禁忌表Bk初始化；
S3、M只蚂蚁从初始点出发；
S4、蚂蚁个体k根据状态转移概率公式以最大转移概率从栅格i转移到邻接栅格j；
S5、蚂蚁个体k每转移一次，将栅格j加入禁忌表Bk；
S6、根据精英策略信息素更新公式τij(t+1)更新信息素，用于状态转移概率公式里τij(t)的更新；其中，τij(t+1)表示t+1时刻蚂蚁个体k从栅格i点转移到栅格j点的信息素强度；
S7、判断所有的蚂蚁是否到达终点：若没有，则重复步骤S4-S6，直至所有的蚂蚁到达终止点；若有，则进行步骤S8；
S8、选出M条路径中距离最短的路径作为蚁群历经所有点后的最优路径并保存；
S9、判断是否达到最大迭代次数Nmax：若没有，则跳转到步骤S3；若达到，则从N条最优路径中选择距离最短的一条作为预规划路径，并执行步骤S10；
S10、引入基于中心点的平滑方法对预规划路径进行平滑操作并输出平滑路径。
进一步地，可以设置所述步骤S2中，蚂蚁个数M为50，迭代次数N为200，启发因子α为1，期望启发因子β为7，信息素蒸发系数ρ为0.7，信息素增加强度系数Q为1，机器人的起始点坐标为(0.5,16.5)，终止点坐标为(16.5,0.5)，期望偏转角度θ1为155°。
进一步地，可以设置所述步骤S10具体为：通过在预规划路径的拐点处增加新节点来实现的，用新的节点代替旧节点来完成路径的平滑处理，是否添加新的节点与拐角的角度期望值α1有关，若两条相邻直线的实际拐角θ2小于等于θ1，则在两条线段的可行域之间取线段的中点(x-new1,y-new1)和(x-new2,y-new2)，再判断新节点与两边构成的拐角角度是否满足大于角度期望值α1：若不满足，继续进行上述变换，寻找新的拐点(x-new1,y-new1)和(x-new2,y-new2)；满足角度条件后，删除旧拐点，以新拐点代替；以此判断路径内的其他拐点是否满足条件，不断重复，使得整条路径的拐角都大于角度期望值。
实施例2：如图1-8所示，一种基于蚁群改进和尖峰平滑的移动机器人路径规划方法，所述方法步骤如下：
步骤一、利用栅格法对移动机器人运行空间进行环境建模，如图2所示，矩阵中1表示黑色格子代表障碍物，0表示白色格子代表自由可通过栅格，划分机器人的工作环境为20×20和17×17栅格；
步骤二、初始化参数，蚂蚁个数M为50，迭代次数N为200，启发因子α为1，期望启发因子β为7，信息素蒸发系数ρ为0.7，信息素增加强度系数Q为1，机器人的起始点坐标为(0.5,16.5)，终止点坐标为(16.5,0.5)，期望偏转角度θ1为155°，禁忌表Bk初始化；
步骤三、M只蚂蚁从初始点出发；
步骤四、蚂蚁个体k根据状态转移概率公式以最大转移概率从栅格i转移到邻接栅格j；其中，k＝1,2,...M；i是正在访问的城市，j是尚未访问的城市。
式中：α：表示启发因子；β：表示期望启发因子；
τij(t)：表示t时刻蚂蚁个体k从栅格i点转移到栅格j点的信息素即轨迹强度；
ηij(t)：表示t时刻蚂蚁个体k从栅格i点转移到栅格j的启发程度，其公式为：dij表示移动机器人所在位置栅格i距离j的距离；
allowedk：表示蚂蚁个体k下一步允许选择的栅格点，allowedk＝D-Bk，D＝(0,1,2，……n-1)表示可选路径节点，Bk表示禁忌表，即蚂蚁k当前已经走过的栅格点；
τis(t)：表示t时刻蚂蚁个体k从栅格i点转移到起始点s点的信息素即轨迹强度，其中t表示t时刻；
ηis(t)：表示t时刻蚂蚁个体k从栅格i点转移到起始点s点的启发程度。
蚂蚁个体k根据状态转移概率找到下一步要走的栅格点。
步骤五、蚂蚁个体k每转移一次，将节点j加入禁忌表Bk；
步骤六、根据精英策略信息素更新公式τij(t+1)更新信息素，用于状态转移概率公式里τij(t)的更新；其中，τij(t+1)表示t+1时刻蚂蚁个体k从栅格i点转移到栅格j点的信息素强度；
其中：
式中，ρ：表示信息素挥发系数；
τij(t)：表示t时刻蚂蚁个体k从栅格i点转移到栅格j点的信息素强度；
Δτij＝Q/Lk表示路段(i,j)上信息素的增量，Lk表示蚂蚁个体k在本次循环中所走的路径总长度；
Δτij*：表示精英蚂蚁在路径(i,j)上信息素的增加；
其中，σ：表示精英蚂蚁及数量；L*：是路径循环结束后所找出的最优解相对应的路径长度。
步骤七、判断所有的蚂蚁是否到达终点。若没有，则重复步骤四到步骤六，直至所有的蚂蚁到达终止点；若有，则进行步骤八；
步骤八、选出蚁群历经所有点后的最优路径并保存；
步骤九、判断是否达到最大迭代次数Nmax。若没有，则跳转到步骤三；若N＝Nmax，则进行步骤十；
步骤十、引入基于中心点的平滑方法对路径进行平滑操作并输出平滑路径。如图3所示，是基于中心点的平滑方法的操作示意图，平滑方法是通过在路径的拐点处增加新节点来实现的，用新的节点代替旧节点来完成路径的平滑处理，节点的选择添加直接影响到路径的平滑和整体路径规划的效率。是否添加新的节点与拐角的角度期望值α1有关，角度期望值α1定为155°，若两条相邻直线的实际拐角α2小于等于α1，则在两条线段的可行域之间取线段的中点(x-new1,y-new1)和(x-new2,y-new2)，再判断新节点与两边构成的拐角角度是否满足大于角度期望值α1，若不满足，继续进行上述变换，寻找新的拐点(x-new1,y-new1)和(x-new2,y-new2)。满足角度条件后，删除旧拐点，以新拐点代替。再以此判断路径内的其他拐点是否满足条件，不断重复，使得整条路径的拐角都大于角度期望值。
为验证本发明该改进蚁群算法的可行性和有效性，分别在20×20和17×17栅格环境下进行仿真，在20×20栅格环境下，传统蚁群算法和本发明改进蚁群算法寻找最优路径结果如图4和图5所示，在17×17栅格时增加障碍物的密集程度，运行结果如图6、图7和图8所示。
如图4和图5所示，可看出，本文提出的路径平滑方法应用于移动机器人路径规划过程中，不仅能够规划出令人满意的避障路径，而且在机器人拐弯处的路径规划有了明显的平滑改善。路径由原来的30.3848缩短为29.7990，迭代次数由32减少到29,，证明了该平滑方法为实体机器人规划出一条更加适合自主移动的平滑路线。
如图6、图7和图8所示对机器人在环境17×17的栅格环境下进行路径规划仿真，参数设置与20×20栅格一致。分别采用传统蚁群算法、基于精英策略的改进蚁群算法、在精英策略的基础上加入平滑方法的改进蚁群算法在17×17的栅格的环境下进行路径规划仿真研究，图6、图7和图8给出了采用传统算法、引入精英策略的改进算法和加入平滑方法的移动机器人路径规划的仿真结果，下表对其路径长度和仿真耗时汇总对比。
由图6可知，传统的蚁群算法(ACA)虽然能有效的避开障碍物并规划出一条可行路线，但由于蚁群算法本身有容易陷入局部最优的缺点，使得路径中出现了不必要的冗余节点，为了去除路径中的冗余节点，缩短路径长度，引入精英策略。如图7所示是引入精英策略的ACA路径规划图，精英策略提高了ACA的全局搜索能力，消除了路径中的多余节点，由表1知，路径长度由原来的33.8995缩短为32.7279，迭代次数由70减少到35，迭代次数减少一半。其次引入中心算子进一步对拐点进行优化，使得路径更加圆滑，符合实体机器人的运动特性，同时路径长度缩短为31.8995，迭代次数减少为25。如图8，与传统ACA路径规划相比，此改进ACA缩短了路径长度，减少了迭代次数，结合了实体机器人对路径规划的特殊要求，得到了一条平滑的路线。
表中的数据显示路径规划在通过精英策略和中心算子的引入后所得路径长度明显优于传统ACA，分别减少了3.46％和5.90％，说明精英策略和基于中心点的平滑方法发挥了其作用。该改进蚁群算法明显提高了路径的平滑度，所获得的路径长度更短。
上面结合附图对本发明的具体实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化。