CN103472828A

CN103472828A - Mobile robot path planning method based on improvement of ant colony algorithm and particle swarm optimization

Info

Publication number: CN103472828A
Application number: CN2013104170083A
Authority: CN
Inventors: 何少佳; 史剑清; 王海坤; 黄知超; 高韵沣; 石旅光; 邓子信
Original assignee: Guilin University of Electronic Technology
Current assignee: Guilin University of Electronic Technology
Priority date: 2013-09-13
Filing date: 2013-09-13
Publication date: 2013-12-25

Abstract

The invention discloses a path planning method for a mobile robot based on an improved ant colony particle swarm algorithm, which mainly solves the problems in the prior art that the algorithm runs at a slow speed and the number of turns of the optimized path is too many. The planning steps are as follows: modeling the working environment of the mobile robot; using the particle swarm algorithm for fast path planning, and spreading more pheromones on the obtained path than the surroundings to provide guidance for the ant colony; using the principle of inertia to optimize the ant colony. The swarm algorithm is optimized on the basis of the particle swarm algorithm; the motion path of the mobile robot is output according to the optimization result. The invention comprehensively considers the stability and robustness of the algorithm, can effectively reduce the number of iterations, improve the search efficiency, shorten the path length, reduce the number of turns, greatly improve the path quality, conform to the intention of manual planning, and is suitable for all kinds of mobile robots in static Autonomous navigation in the environment.

Description

Path Planning Method for Mobile Robot Based on Improved Ant Colony Particle Swarm Algorithm

技术领域technical field

本发明涉及移动机器人技术领域，特别是在全局静态环境下的基于改进蚁群粒子群算法的移动机器人路径规划方法。The invention relates to the technical field of mobile robots, in particular to a path planning method for a mobile robot based on an improved ant colony particle swarm algorithm in a global static environment.

背景技术Background technique

路径规划是移动机器人的关键技术之一，它在一定程度上标志着移动机器人智能水平的高低，能快速找出一条便捷、无碰撞的路径不仅保证了移动机器人自身的安全，更体现了机器人的高效性和可靠性。Path planning is one of the key technologies of mobile robots. To a certain extent, it marks the intelligence level of mobile robots. Being able to quickly find a convenient and collision-free path not only ensures the safety of the mobile robot itself, but also reflects the robot's Efficiency and reliability.

目前，常用到的路径规划方法有可视图法、启发式图搜索算法、人工势场法、A*算法等。这些算法有各自的优缺点，例如人工势场法具有良好的实时性，但存在陷阱区域，并且在相近障碍物之间不能发现路径等缺点，A*算法更适用于解决单目标优化问题。近十年间，随着人工智能算法的研究不断取得进展，许多智能算法也被用到移动机器人的路径规划中，包括模糊逻辑与增强学习算法、神经网络、遗传算法以及蚁群算法等。这些算法都有各自的优点，但也存在诸多问题，例如算法复杂、局部最优、搜索空间过大等。这些算法对硬件条件要求较高，并且不满足移动机器人实时性的要求。At present, the commonly used path planning methods include visual graph method, heuristic graph search algorithm, artificial potential field method, A* algorithm, etc. These algorithms have their own advantages and disadvantages. For example, the artificial potential field method has good real-time performance, but there are trap areas, and the path cannot be found between similar obstacles. The A* algorithm is more suitable for solving single-objective optimization problems. In the past ten years, with the continuous progress in the research of artificial intelligence algorithms, many intelligent algorithms have also been used in the path planning of mobile robots, including fuzzy logic and reinforcement learning algorithms, neural networks, genetic algorithms, and ant colony algorithms. These algorithms have their own advantages, but there are also many problems, such as algorithm complexity, local optimum, too large search space and so on. These algorithms have high requirements on hardware conditions, and do not meet the real-time requirements of mobile robots.

虽然现阶段已对诸多算法进行改进，能够较好地找出最优路径，但是仍然存在迭代次数较多、运算时间过长等问题，这些问题不能很好的满足移动机器人实时性的要求；而且，在得到的路径中存在转弯次数较多的情况，这将严重影响移动机器人的工作效率，降低工作可靠性。Although many algorithms have been improved at this stage, and the optimal path can be better found, there are still problems such as a large number of iterations and too long calculation time, which cannot well meet the real-time requirements of mobile robots; and , there are many turns in the obtained path, which will seriously affect the work efficiency of the mobile robot and reduce the work reliability.

发明内容Contents of the invention

本发明的目的是针对现有技术的不足，提出一种改进蚁群粒子群优化算法，能有效减少迭代次数，提高搜索效率，缩短路径长度，减少转弯次数，大幅提高路径质量，符合人工规划意图，适用于各类移动机器人在静态环境下的自主导航。The purpose of the present invention is to address the deficiencies in the prior art and propose an improved ant colony particle swarm optimization algorithm, which can effectively reduce the number of iterations, improve search efficiency, shorten the path length, reduce the number of turns, greatly improve the path quality, and meet the intention of manual planning , which is suitable for autonomous navigation of various mobile robots in static environments.

本发明主要由三部分组成：第一部分先利用粒子群算法得到蚁群算法所需要的初始信息分布；第二部分采用传统蚁群算法，但是每次迭代只记录爬行路线不释放信息素；第三部分对每条爬行路线进行惯性优化，然后进行信息素更新。The present invention is mainly composed of three parts: the first part first uses the particle swarm algorithm to obtain the initial information distribution required by the ant colony algorithm; the second part uses the traditional ant colony algorithm, but only records the crawling route and does not release pheromone in each iteration; the third Each crawl route is partially inertial optimized followed by pheromone updates.

实现本发明目的的技术方案是：The technical scheme that realizes the object of the present invention is:

基于改进蚁群粒子群算法的移动机器人路径规划方法，包含如下步骤：A mobile robot path planning method based on the improved ant colony particle swarm algorithm, including the following steps:

步骤一：利用机器人自带的摄像头、超声波传感器、红外传感器采集移动机器人工作环境信息，包括起始点、目标点、障碍物，并进行栅格地图建模，对建模要求如下：(1)移动机器人的活动范围在一个有限的二维空间；(2)以移动机器人的尺寸为基准，将障碍物的尺寸向外扩展，将机器人看作一个质点；(3)障碍物由任意栅格方块组成，数目有限，并且在机器人移动过程中这些障碍物不会发生变化和移动。Step 1: Use the robot's own camera, ultrasonic sensor, and infrared sensor to collect the working environment information of the mobile robot, including the starting point, target point, and obstacles, and perform grid map modeling. The modeling requirements are as follows: (1) Mobile The range of activities of the robot is in a limited two-dimensional space; (2) Based on the size of the mobile robot, the size of the obstacle is expanded outward, and the robot is regarded as a particle; (3) The obstacle is composed of any grid square , the number is limited, and these obstacles will not change and move during the movement of the robot.

步骤二：采用粒子群算法进行快速路径寻优，将得到的路径转化为蚁群算法的初始信息素分布信息，具体包括：Step 2: Use the particle swarm optimization algorithm for fast path optimization, and convert the obtained path into the initial pheromone distribution information of the ant colony algorithm, specifically including:

2.1对环境模型进行编码处理；2.1 Coding the environment model;

2.2设置初始参数，种群规模N_z，惯性权重ω，学习因子c₁和c₂，最大迭代次数M_z，速度最大值V_max；2.2 Set initial parameters, population size N _z , inertia weight ω, learning factors c ₁ and c ₂ , maximum number of iterations M _z , maximum speed V _max ;

2.3随机生成粒子i的位置矢量Z_i=(z_i1，z_i2，…，z_iD)和速度矢量v_i=(v_i1，v_i2，…，v_iD)，初始化粒子历史最优值p_i和全局最优值p_g；2.3 Randomly generate the position vector Z _i =(z _i1 , z _i2 ,…, z _iD ) and velocity vector v _i =(v _i1 ,v _i2 ,…,v _iD ) of particle i, and initialize the particle’s historical optimal value p _i and the global optimal value p _g ;

2.4计算每个粒子当前适度值f(z_i)。如果f(z_i)<f(p_i)，则p_i=z_i，f(z_j)=minf(p_i)，如果f(z_j)<f(p_g)，则p_g=z_j；适度值函数为：2.4 Calculate the current fitness value f(z _i ) of each particle. If f(z _i )<f(p _i ), then p _i =z _i , f(z _j )=minf(p _i ), if f(z _j )<f(p _g ), then p _g =z _j ; the moderate value function is:

$f f = = {Σ Σ}_{m m = = 22}^{n no} \sqrt{{(({x x}_{m m} - - {x x}_{m m - - 11}))}^{22} + + {(({y the y}_{m m} - - {y the y}_{m m - - 11}))}^{22}}$

式中(x_m，y_m)为路径点坐标信息，n为路径点个数。Where (x _m , y _m ) is the coordinate information of the waypoint, and n is the number of waypoints.

2.5更新粒子位置信息和速度信息，分别按以下两式进行：2.5 Update particle position information and velocity information, respectively according to the following two formulas:

${v v}_{id id}^{k k + + 11} = = {ωv ω v}_{id id}^{k k} + + {c c}_{11} {r r}_{11} (({p p}_{id id} - - {z z}_{id id}^{k k})) + + {c c}_{22} {r r}_{22} (({p p}_{gd gd} - - {z z}_{id id}^{k k}))$

${z z}_{id id}^{k k + + 11} = = {z z}_{id id}^{k k} + + {v v}_{id id}^{k k + + 11}$

式中，i=1，2，…，N；d=1，2，…，D；k为迭代次数，ω为惯性权重，c₁和c₂为学习因子，r₁和r₂是[0，1]之间的相互独立、均匀分布的随机数，v_i＝(v_i1，v_i2，…，v_iD)为粒子i的飞行速度，也是粒子移动的距离，p_i=(p_i1，p_i2，…，p_iD)为粒子迄今为止搜索到的最优位置，p_g=(p_g1，p_g2,…，p_gD)为整个粒子群迄今为止搜索到的最优位置，p_g=min(p_i1，p_i2，…，p_iD)。In the formula, i=1, 2,..., N; d=1, 2,..., D; k is the number of iterations, ω is the inertia weight, c ₁ and c ₂ are learning factors, r ₁ and r ₂ are [0 , 1] are mutually independent and uniformly distributed random numbers, v _i =(v _i1 , v _i2 ,..., v _iD ) is the flight speed of particle i, and is also the moving distance of particle i, p _i =(p _i1 , p _i2 ,..., p _iD ) is the optimal position searched by the particle so far, p _g = (p _g1 , p _g2 ,..., p _gD ) is the optimal position searched by the entire particle swarm so far, p _g = min(p _i1 , p _i2 , . . . , p _iD ).

2.6转到步骤2.4，直至达到最大迭代次数或满足精度要求。2.6 Go to step 2.4 until the maximum number of iterations is reached or the accuracy requirement is met.

步骤三：采用惯性原理优化后的蚁群算法，进行路径寻优，具体包括：Step 3: Use the ant colony algorithm optimized by the principle of inertia to optimize the path, including:

3.1设置蚂蚁数量N_a，最大迭代次数M_a，利用PSO算法得到的最优路径p_g，按照下式初始化信息素信息，并将全部蚂蚁置于起始点；3.1 Set the number of ants N _a , the maximum number of iterations M _a , use the optimal path p _g obtained by the PSO algorithm, initialize the pheromone information according to the following formula, and place all ants at the starting point;

${τ τ}_{ij ij} = = {τ τ}_{ij ij}^{s the s} + + Δ Δ {τ τ}_{ij ij}$

式中τ_ij为信息素浓度，τ^s _ij为环境地图信息得到信息素的初始值，Δτ_ij为PSO算法得到的最优路径转换为信息素的增强变量；In the formula, τ _ij is the concentration of pheromone, τ ^s _ij is the initial value of pheromone obtained from environmental map information, and Δτ _ij is the enhanced variable converted into pheromone from the optimal path obtained by PSO algorithm;

3.2启动蚁群，蚂蚁使用轮盘赌法进行路径点选择，并记录路径点信息；轮盘赌法公式如下：3.2 Start the ant colony, and the ants use the roulette method to select the waypoint and record the waypoint information; the formula of the roulette method is as follows:

${P P}_{ij ij} ((t t)) = = \frac{{τ τ}_{ij ij}^{α α} ((t t)) {η η}_{ij ij}^{β β} ((t t))}{{Σ Σ}_{j j &Element; &Element; M m} {τ τ}_{ij ij}^{α α} ((t t)) {η η}_{ij ij}^{β β} ((t t))}$

式中：P为选择下个路径点概率，τ_ij为信息素浓度，η_ij为栅格i和栅格j之间距离的倒数，α为信息素的相对重要性，β为距离的相对重要性，M为下一步可选栅格集合；In the formula: P is the probability of selecting the next path point, τ _ij is the pheromone concentration, η _ij is the reciprocal of the distance between grid i and grid j, α is the relative importance of pheromone, β is the relative importance of distance , M is the optional raster set for the next step;

3.3重复步骤3.2，直至所有蚂蚁达到目标点；3.3 Repeat step 3.2 until all ants reach the target point;

3.4对得到的路径进行惯性优化，保存优化后的路径信息。采用惯性优化原理对算法进行优化，以解决路线折线过多、转折次数过多的问题。假设G_i(x_i，y_i)为当前路径点，G_i-1(x_i-1，y_i-1)为上一个路径点，G_i+1(x_i-1，y_i+1)为下一个路径点(是G_i周围待选取的8个路径点之一)，G_j(x_j，y_j)为目标点，具体公式如下：3.4 Perform inertial optimization on the obtained path, and save the optimized path information. The algorithm is optimized by using the principle of inertial optimization to solve the problem of too many broken lines and too many turning times. Suppose G _i (xi _-1 , y _i ) is the current path point, G _i-1 (xi _-1 , y _i-1 ) is the previous path point, G _i+1 (xi _-1 , y _i+1 ) is the next path point (one of the eight path points to be selected around G _i ), G _j (x _j , y _j ) is the target point, and the specific formula is as follows:

式中s为两个路径点之间的间距，C为常数，式中惯性点是指与G_i、G_i-1在同一直线上的G_i+1，需满足式：In the formula, s is the distance between two path points, and C is a constant. In the formula, the inertia point refers to G _i+1 on the same straight line as G _i and G _i- 1, which must satisfy the formula:

$\{\begin{matrix} {x x}_{i i + + 11} - - {x x}_{i i} = = {x x}_{i i} - - {x x}_{i i - - 11} \\ {y the y}_{i i + + 11} - - {y the y}_{i i} = = {y the y}_{i i} - - {y the y}_{i i - - 11} \end{matrix}$

如果G_i+1也属于惯性点，那么用于计算s的(x’_i+1，y’_i+1)需要调整为：If G _i+1 also belongs to the inertial point, then the (x' _i+1 , y' _i+1 ) used to calculate s needs to be adjusted to:

$\{\begin{matrix} {x x}_{i i + + 11}^{' '} = = {33 x x}_{i i} - - {22 x x}_{i i - - 11} \\ {y the y}_{i i + + 11}^{' '} = = {33 y the y}_{i i} - - {22 y the y}_{i i - - 11} \end{matrix}$

3.5更新得到路径上的信息素。传统的蚁群算法是对所有蚂蚁走过的路径都进行信息素更新，这种机制有一定的缺陷，会减慢算法的收敛速度，降低搜索效率。本发明采取的机制是：对蚁群中的周游最优蚂蚁和全局最优蚂蚁的路径信息进行信息素更新，从而加强了反馈效果，增加解的多样性，减小陷入局部最优的概率。更新的机制如下式所示：3.5 update to get the pheromone on the path. The traditional ant colony algorithm is to update the pheromones of all the paths that ants have traveled. This mechanism has certain defects, which will slow down the convergence speed of the algorithm and reduce the search efficiency. The mechanism adopted by the present invention is to update the pheromones of the path information of the roaming optimal ant and the global optimal ant in the ant colony, thereby strengthening the feedback effect, increasing the diversity of solutions, and reducing the probability of falling into local optimal. The updated mechanism is as follows:

τ_ij(t+1)＝ρτ_ij(t)+Δτ_ij τ _ij (t+1)=ρτ _ij (t)+Δτ _ij

式中：ρ表示信息素的挥发系数，Δτ_ij表示本次循环的信息素增量，表达式如下：In the formula: ρ represents the volatilization coefficient of pheromone, Δτ _ij represents the pheromone increment of this cycle, and the expression is as follows:

$Δ Δ {τ τ}_{ij ij} = = ((11 - - ρ ρ)) \times \times ((\frac{{a a}_{k k}}{{L L}_{c c}} + + \frac{{b b}_{k k}}{{L L}_{ω ω}})) \times \times \frac{Q Q}{{a a}_{k k} + + {b b}_{k k}}$

其中L_C为周游最优蚂蚁走过的路径长度，L_ω为全局最优蚂蚁走过的路径长度，a_k和b_k为整型变量，分别代表周游最优蚂蚁和全局最优蚂蚁跟新信息素的权重，Q为常数；Among them, L _C is the path length traveled by the optimal ant in roaming, L _ω is the path length traveled by the optimal ant in the world, a _k and b _k are integer variables, which respectively represent the optimal ant in roaming and the global optimal ant to follow the new path. The weight of pheromone, Q is a constant;

3.6转跳至步骤2，直至搜索路径不在变化或达到最大迭代次数；3.6 Jump to step 2 until the search path does not change or reaches the maximum number of iterations;

3.7输出最优路径。3.7 Output the optimal path.

在整个步骤三中，信息素的挥发系数ρ对在复杂地图中搜索有着重要作用，如果ρ过大，会降低算法收敛速度，如果ρ过小，则易陷入局部最优。本发明采用动态调整方法，如下所示：In the third step, the volatility coefficient ρ of pheromone plays an important role in searching in complex maps. If ρ is too large, the convergence speed of the algorithm will be reduced. If ρ is too small, it will easily fall into local optimum. The present invention adopts dynamic adjustment method, as follows:

$ρ ρ ((t t)) = = \frac{{L L}_{mid middle} - - {L L}_{min min}}{{L L}_{max max} - - {L L}_{min min}}$

式中：L_max表示最长优化路径，L_mid表示平均优化路径，L_min表示最短优化路径。In the formula: L _max represents the longest optimization path, L _mid represents the average optimization path, and L _min represents the shortest optimization path.

步骤四：根据优化结果输出规划路径。Step 4: Output the planned path according to the optimization result.

本发明方法的优点和积极效果在于：Advantage and positive effect of the inventive method are:

(1)采用栅格地图建模方式，具有精度高、易于实现等优点；(1) The grid map modeling method is adopted, which has the advantages of high precision and easy implementation;

(2)将粒子群算法与蚁群算法融合起来。粒子群算法的优势在于它相对于蚁群算法来说所需时间很短，加在蚁群算法之前不仅不会消耗太多时间、影响整体运行速率，而且能为蚁群算法提供初期信息素反馈，这样能减少蚁群算法的运行时间，从而达到即能减少搜索时间又得到优化路径的效果；(2) Combine particle swarm algorithm and ant colony algorithm. The advantage of the particle swarm optimization algorithm is that it takes a very short time compared to the ant colony algorithm. It will not only consume too much time and affect the overall running speed before the ant colony algorithm, but also provide initial pheromone feedback for the ant colony algorithm , which can reduce the running time of the ant colony algorithm, so as to achieve the effect of reducing the search time and obtaining the optimized path;

(3)采用惯性原理优化整个融合算法，能有效减少转弯次数，提高移动机器人的工作效率和工作可靠性；(3) Using the principle of inertia to optimize the entire fusion algorithm can effectively reduce the number of turns and improve the work efficiency and reliability of the mobile robot;

(4)动态调整信息素的挥发系数ρ，有效保证了算法的运行质量。(4) Dynamically adjust the volatilization coefficient ρ of the pheromone to effectively ensure the running quality of the algorithm.

附图说明Description of drawings

图1为本发明惯性优化流程图；Fig. 1 is the flow chart of inertial optimization of the present invention;

图2为本发明对工作环境建模图；Fig. 2 is the working environment modeling figure of the present invention;

图3为本发明对环境地图编码处理图；Fig. 3 is the encoding processing diagram of the environment map in the present invention;

图4为一条PSO快速路径规划结果图；Fig. 4 is a PSO fast path planning result diagram;

图5(a)-(b)为在简单环境下本发明方法与其他方法寻优路径结果对比图；Figure 5(a)-(b) is a comparison diagram of the results of the method of the present invention and other methods for optimizing the path in a simple environment;

图6(a)-(b)为在复杂环境下本发明方法与其他方法寻优路径结果对比图。Figure 6(a)-(b) is a comparison of the results of the method of the present invention and other methods for path optimization in a complex environment.

具体实施方式Detailed ways

下面将结合附图和实施例对本发明内容进行详细说明，但不是对本发明的限定。The content of the present invention will be described in detail below with reference to the drawings and embodiments, but the present invention is not limited thereto.

本发明一种基于改进蚁群粒子群算法的移动机器人路径规划方法，具体包括以下步骤：A kind of mobile robot path planning method based on improved ant colony particle swarm algorithm of the present invention, specifically comprises the following steps:

步骤一：环境建模：Step 1: Environment Modeling:

移动机器人的路径规划是在工作环境中找出一个从起始点到目标点的点的序列。为不失一般性，对移动机器人的工作空间做出以下规定：(1)移动机器人的活动范围在一个有限的二维空间；(2)以移动机器人的尺寸为基准，将障碍物的尺寸向外扩展，将机器人看作一个质点；(3)障碍物由任意栅格方块组成，数目有限，并且在机器人移动过程中这些障碍物不会发生变化和移动。Path planning for a mobile robot is to find a sequence of points in the working environment from a starting point to a goal point. Without loss of generality, the following regulations are made on the working space of the mobile robot: (1) The range of activities of the mobile robot is in a limited two-dimensional space; (2) Based on the size of the mobile robot, the size of the obstacle is Expand outward, regard the robot as a mass point; (3) Obstacles are composed of arbitrary grid squares, the number is limited, and these obstacles will not change and move during the movement of the robot.

如图2所示，一个工作环境建模图，图中黑色栅格为障碍栅格，表示障碍物；白色栅格表示自由区域，可以供机器人移动；S表示起始点；G表示目标点。每一个栅格的长宽都为1个单位，任意两个栅格间的距离是它们中心点的连线距离，记作l：As shown in Figure 2, a working environment modeling diagram, the black grid in the figure is the obstacle grid, indicating obstacles; the white grid indicates the free area, which can be moved by the robot; S indicates the starting point; G indicates the target point. The length and width of each grid is 1 unit, and the distance between any two grids is the line distance between their center points, denoted as l:

$l l = = \sqrt{{(({x x}_{i i} - - {x x}_{j j}))}^{22} + + {(({y the y}_{i i} - - {y the y}_{j j}))}^{22}}$

总路径长度为L：The total path length is L:

$L L = = {Σ Σ}_{m m = = 22}^{n no} \sqrt{{(({x x}_{m m} - - {x x}_{m m - - 11}))}^{22} + + {(({y the y}_{m m} - - {y the y}_{m m - - 11}))}^{22}}$

式中(x_m，y_m)为路径点坐标信息，n为路径点个数。L亦为目标函数。In the formula, (x _m , y _m ) is the coordinate information of the waypoint, and n is the number of waypoints. L is also the objective function.

步骤二：采用粒子群算法进行快速路径寻优，其具体过程包括：Step 2: Use particle swarm optimization algorithm for fast path optimization, the specific process includes:

A、对环境地图进行编码处理。A. Coding the environment map.

如图3所示，首先把起始点和目标点连接起来，然后将其进行等分，图中虚线是在每个等分点做的垂线，虚线经过的白色栅格的编号作为粒子编码，粒子正是在这些编码中随机选择。如果选择出的相邻两个编码所代表的白色栅格之间的连线不穿过黑色栅格，那么就可以将其作为一条待选路径。图4为一条规划得到的路径。As shown in Figure 3, first connect the starting point and the target point, and then divide it equally. The dotted line in the figure is the vertical line made at each equalization point, and the number of the white grid that the dotted line passes is used as the particle code. It is within these codes that the particles are randomly selected. If the selected line between the white grids represented by two adjacent codes does not pass through the black grid, then it can be used as a path to be selected. Figure 4 shows a planned path.

B、设置初始参数，种群规模N_z=25，最大迭代次数M_z=50，学习因子c₁＝c₂＝1.45，惯性权重ω从0.8随迭代次数线性递减到0.4，速度最大值V_max=10；B. Set initial parameters, population size N _z =25, maximum number of iterations M _z =50, learning factor c ₁ =c ₂ =1.45, inertia weight ω linearly decreases from 0.8 to 0.4 with the number of iterations, maximum speed V _max = 10;

C、随机生成粒子i的位置矢量Z_i=(z_i1，z_i2，…，z_iD)和速度矢量v_i=(v_i1，v_i2，…，v_iD)，初始化粒子历史最优值p_i和全局最优值p_g；C. Randomly generate the position vector Z _i =(z _i1 , z _i2 ,..., z _iD ) and velocity vector v _i =(v _i1 , v _i2 ,..., v _iD ) of particle i, and initialize the particle's historical optimal value p _i and the global optimal value p _g ;

D、计算每个粒子当前适度值f(z_i)。如果f(z_i)<f(p_i)，则p_i=z_i，f（z_j)=min f(p_i)，如果f(z_j)<f(p_g)，则p_g=z_j；适度值函数为：D. Calculate the current fitness value f(z _i ) of each particle. If f(z _i )<f(p _i ), then p _i =z _i , f(z _j )=min f(p _i ), if f(z _j )<f(p _g ), then p _g = z _j ; the moderate value function is:

式中(x_m，y_m)为路径点坐标信息，n为路径点个数。In the formula, (x _m , y _m ) is the coordinate information of the waypoint, and n is the number of waypoints.

E、更新粒子位置信息和速度信息，分别按以下两式进行：E. Update particle position information and velocity information, respectively according to the following two formulas:

式中，i=1，2，…，N；d=1，2，…，D；k为迭代次数，ω为惯性权重，c₁和c₂为学习因子，r₁和r₂是[0，1]之间的相互独立、均匀分布的随机数，v_i=(v_i1，v_i2，…，v_iD)为粒子i的飞行速度，也是粒子移动的距离，p_i＝(p_i1，p_i2，…，p_iD)为粒子迄今为止搜索到的最优位置，p_g=(p_g1，p_g2，…，p_gD)为整个粒子群迄今为止搜索到的最优位置，p_g=min(p_i1，p_i2，…，p_iD)。In the formula, i=1, 2,..., N; d=1, 2,..., D; k is the number of iterations, ω is the inertia weight, c ₁ and c ₂ are learning factors, r ₁ and r ₂ are [0 , 1] are mutually independent and uniformly distributed random numbers, v _i =(v _i1 , v _i2 ,..., v _iD ) is the flight speed of particle i, and is also the moving distance of particle i, p _i =(p _i1 , p _i2 ,..., p _iD ) is the optimal position searched by the particle so far, p _g = (p _g1 , p _g2 , ..., p _gD ) is the optimal position searched by the entire particle swarm so far, p _g = min(p _i1 , p _i2 , . . . , p _iD ).

F、转到步骤D，直至达到最大迭代次数或满足精度要求。F. Go to step D until the maximum number of iterations is reached or the accuracy requirement is met.

步骤三：采用惯性原理优化后的蚁群算法，进行寻优，具体包括：Step 3: Use the ant colony algorithm optimized by the principle of inertia to perform optimization, including:

I设置蚂蚁数量N_a＝20，最大迭代次数M_a＝100，利用PSO算法得到的最优路径p_g，按照下式初始化信息素信息，并将全部蚂蚁置于起始点；I set the number of ants N _a =20, the maximum number of iterations M _a =100, use the optimal path p _g obtained by the PSO algorithm, initialize the pheromone information according to the following formula, and place all the ants at the starting point;

${τ τ}_{ij ij} = = {τ τ}_{ij ij}^{s the s} + + Δ Δ {τ τ}_{ij ij}$

式中式中τ_ij信息素浓度，τ^s _ij为环境地图信息得到信息素的初始值，Δτ_ij为PSO算法得到的最优路径转换为信息素的增强变量；In the formula, τ _ij pheromone concentration, τ ^s _ij is the initial value of pheromone obtained from environmental map information, Δτ _ij is the enhanced variable converted into pheromone from the optimal path obtained by PSO algorithm;

II启动蚁群，蚂蚁使用轮盘赌法进行路径点选择，并记录路径点信息；轮盘赌法公式如下：II starts the ant colony, and the ants use the roulette method to select the waypoint and record the waypoint information; the formula of the roulette method is as follows:

式中：P为选择下个路径点概率，τ_ij为信息素浓度，η_ij为栅格i和栅格j之间距离的倒数，α为信息素的相对重要性，取值1.5，β为距离的相对重要性，取值2.0，M为下一步可选栅格集合。In the formula: P is the probability of selecting the next path point, τ _ij is the pheromone concentration, η _ij is the reciprocal of the distance between grid i and grid j, α is the relative importance of pheromone, the value is 1.5, β is The relative importance of the distance, the value is 2.0, and M is the optional raster set for the next step.

III重复步骤3.2，直至所有蚂蚁达到目标点；III Repeat step 3.2 until all ants reach the target point;

Ⅳ对得到的路径进行惯性优化，保存优化后的路径信息。采用惯性优化原理对算法进行优化，以解决路线折线过多、转折次数过多的问题。假设G_i(x_i，y_i)为当前路径点，G_i-1(x_i-1，y_i-1)为上一个路径点，G_j+1(x_i-1，y_i+1)为下一个路径点(是G_i周围待选取的8个路径点之一)，G_j(x_j，y_j)为目标点，具体公式如下：Ⅳ Perform inertial optimization on the obtained path, and save the optimized path information. The algorithm is optimized by using the principle of inertial optimization to solve the problem of too many broken lines and too many turning times. Suppose G _i (xi _-1 , y _i ) is the current path point, G _i-1 (xi _-1 , y _i-1 ) is the previous path point, G _j+1 (xi _-1 , y _i+1 ) is the next path point (one of the eight path points to be selected around G _i ), G _j (x _j , y _j ) is the target point, and the specific formula is as follows:

$\{\begin{matrix} {x x}_{i i + + 11}^{' '} = = 33 {x x}_{i i} - - 22 {x x}_{i i - - 11} \\ {y the y}_{i i + + 11}^{' '} = = 33 {y the y}_{i i} - - 22 {y the y}_{i i - - 11} \end{matrix}$

如图1所示，惯性优化流程具体步骤：As shown in Figure 1, the specific steps of the inertial optimization process:

S101：以i为起始点，j为目标点，开始优化；S101: Start optimization with i as the starting point and j as the target point;

S102：按照上述惯性优化原理寻找一条到点j的最短路径，跳转到S103；S102: Find a shortest path to point j according to the above inertial optimization principle, and jump to S103;

S103：判断两点中间是否有障碍物，如果有障碍物，跳转到S104，如果没有障碍物跳转到S108；S103: Determine whether there is an obstacle between the two points, if there is an obstacle, go to S104, if there is no obstacle, go to S108;

S104：把节点j-1赋给j，跳转到S105；S104: assign node j-1 to j, and jump to S105;

S105：判断i与j-1之间大小，如果i＜j-1，跳转到S102，如果i≥j-1，跳转到S106；S105: Determine the size between i and j-1, if i<j-1, jump to S102, if i≥j-1, jump to S106;

S106：选取下一个节点赋值给i，把新的路径终点赋给j，转跳到S107S106: select the next node and assign it to i, assign the new path end point to j, and skip to S107

S107：判断i与j-1之间大小，如果i＜j-1，跳转到S102，如果i≥j-1，跳转到S109；S107: Determine the size between i and j-1, if i<j-1, go to S102, if i≥j-1, go to S109;

S108：删除从i到j之间的原有节点，加入优化后从i到j之间的节点，跳转到S106；S108: delete the original node from i to j, add the optimized node from i to j, and jump to S106;

S109：循环结束。S109: the loop ends.

V更新得到路径上的信息素。传统的蚁群算法是对所有蚂蚁走过的路径都进行信息素更新，这种机制有一定的缺陷，会减慢算法的收敛速度，降低搜索效率。本发明采取的机制是：对蚁群中的周游最优蚂蚁和全局最优蚂蚁的路径信息进行信息素更新，从而加强了反馈效果，增加解的多样性，减小陷入局部最优的概率。更新的机制如下式所示：V updates to get the pheromones on the path. The traditional ant colony algorithm is to update the pheromones of all the paths that ants have traveled. This mechanism has certain defects, which will slow down the convergence speed of the algorithm and reduce the search efficiency. The mechanism adopted by the present invention is to update the pheromones of the path information of the roaming optimal ant and the global optimal ant in the ant colony, thereby strengthening the feedback effect, increasing the diversity of solutions, and reducing the probability of falling into local optimal. The updated mechanism is as follows:

τ_ij(t+1)=ρτ_ij(t)+Δτ_ij τ _ij (t+1)=ρτ _ij (t)+Δτ _ij

其中L_C为周游最优蚂蚁走过的路径长度，L_ω为全局最优蚂蚁走过的路径长度，a_k和b_k为整型变量，分别代表周游最优蚂蚁和全局最优蚂蚁跟新信息素的权重，Q=100；Among them, L _C is the path length traveled by the optimal ant in roaming, L _ω is the path length traveled by the optimal ant in the world, a _k and b _k are integer variables, which respectively represent the optimal ant in roaming and the global optimal ant to follow the new path. The weight of pheromone, Q=100;

Ⅵ转跳至步骤II，直至搜索路径不在变化或达到最大迭代次数；Ⅵ Jump to step II until the search path does not change or reaches the maximum number of iterations;

Ⅶ输出最优路径。VII outputs the optimal path.

在简单环境下进行仿真对比实验。环境为20×20的栅格地图，效果对比图如图5所示：图中，(a)为普通蚁群粒子算法寻优路径图，为方便起见，称普通蚁群粒子算法为ACO+PSO（ant colony algorithm＋particle swarmoptimization），图中路径点数23，转折次数12，路径长度28.63；(b)为本发明算法寻优路径图，为方便起见，称本发明方法为IAC+PSO(improved ant colony＋particle swarm optimization），图中路径点数22，转折次数7，路径长度28.04，相比之下本发明算法优化后的路径质量更高。Carry out simulation comparison experiments in a simple environment. The environment is a 20×20 grid map, and the effect comparison diagram is shown in Figure 5: in the figure, (a) is the optimization path diagram of the common ant colony particle algorithm. For convenience, the common ant colony particle algorithm is called ACO+PSO (ant colony algorithm + particle swarm optimization), the number of path points in the figure is 23, the number of turning times is 12, and the path length is 28.63; (b) is the optimal path diagram of the algorithm of the present invention. For convenience, the method of the present invention is called IAC+PSO (improved ant colony+particle swarm optimization), the number of path points in the figure is 22, the number of turning times is 7, and the path length is 28.04. In contrast, the path quality optimized by the algorithm of the present invention is higher.

在复杂环境下进行仿真对比实验。环境为40×40的栅格地图，效果对比图如图6所示：图中，(a)为ACO+PSO算法，路径长度62.01；(b)为IAC+PSO算法，路径长度58.08。首先本发明算法在长度上比ACO+PSO算法减少了6.3％，可以看出本发明算法寻出的路径更短；其次，ACO+PSO算法明显存在较多弯路，转折点多达27个，而本发明算法只有11个，优化比例59.3％，可以看出本发明算法在获取的路径质量上明显优于ACO+PSO算法。Simulation and comparison experiments are carried out in complex environments. The environment is a 40×40 grid map, and the effect comparison diagram is shown in Figure 6: in the figure, (a) is the ACO+PSO algorithm with a path length of 62.01; (b) is the IAC+PSO algorithm with a path length of 58.08. At first the algorithm of the present invention has reduced 6.3% than ACO+PSO algorithm in length, and it can be seen that the path that the algorithm of the present invention seeks out is shorter; There are only 11 invented algorithms, and the optimization ratio is 59.3%. It can be seen that the obtained path quality of the inventive algorithm is obviously better than that of the ACO+PSO algorithm.

通过多次仿真实验，本发明算法对于一般环境下的移动机器人路径规划，能以很大概率搜索到全局最优路径，并且收敛速度快；对于复杂环境下的路径规划，算法收敛速度有所减慢，但仍然优于其他算法，得到的最优路径长度及路径质量明显提高，并且地图越大优势越明显。下表为IAC+PSO算法、ACO+PSO算法、ACO算法(蚁群算法)在三种不同栅格地图下多次实验数据的平均值。Through multiple simulation experiments, the algorithm of the present invention can search for the global optimal path with a high probability for the path planning of the mobile robot in the general environment, and the convergence speed is fast; for the path planning in the complex environment, the algorithm convergence speed is reduced Slow, but still superior to other algorithms, the optimal path length and path quality obtained are significantly improved, and the larger the map, the more obvious the advantage. The following table shows the average value of multiple experimental data of IAC+PSO algorithm, ACO+PSO algorithm, and ACO algorithm (ant colony algorithm) under three different grid maps.

不同环境下各种算法效果对比Comparison of the effects of various algorithms in different environments

Claims

1. the method for planning path for mobile robot based on improving ant group particle cluster algorithm comprises the following steps:

(01) camera that utilizes robot to carry, ultrasonic sensor, infrared sensor gather the mobile work robot environmental information, comprise starting point, impact point, barrier, and carry out the grating map modeling;

(02) adopt particle cluster algorithm to carry out the fast path optimizing, the path obtained is converted into to the initial information element distributed intelligence of ant group algorithm;

(03) adopt the ant group algorithm after principle of inertia is optimized, carry out optimum path search;

(04) according to optimum results output path planning.

2. the method for planning path for mobile robot based on improving ant group particle cluster algorithm according to claim 1 is characterized in that the detailed process of step (02) comprising:

A, to the environmental model processing of encoding;

B, initial parameter is set, population scale, inertia weight, the study factor, maximum iteration time, speed maximal value;

C, the position vector that generates at random particle and velocity, the historical optimal value of initialization particle p _iand global optimum p _g;

D, calculate the current appropriateness of each particle value;

E, renewal particle position information and velocity information;

F, forward D to, until reach maximum iteration time or meet accuracy requirement.

3. the method for planning path for mobile robot based on improving ant group particle cluster algorithm according to claim 1 is characterized in that the detailed process of step (03) comprising:

I, ant quantity is set, maximum iteration time, the optimal path initialization information prime information of utilizing the PSO algorithm to obtain, and whole ants are placed in to starting point;

II, startup ant group, every ant carries out the path point selection by roulette method, and the record path dot information;

III, repeating step II, until all ants reach impact point;

IV, inertia optimization is carried out in the path obtained, preserve the routing information after optimizing;

V, the pheromones on new route more;

VI, turn and skip to the step II, until searching route is not changing or reaching maximum iteration time;

VII, output optimal path.

4. the method for planning path for mobile robot based on improving ant group particle cluster algorithm according to claim 3, is characterized in that: in the process I, Environmental Map Information is obtained to the initial value of pheromones τ ^s _ijwith pSOthe optimal path that algorithm obtains is converted to the enhancing variable of pheromones Δ τ _ijby following formula, merged:

Figure 2013104170083100001DEST_PATH_IMAGE001

In formula τ _ijfor pheromone concentration.

5. the method for planning path for mobile robot based on improving ant group particle cluster algorithm according to claim 3, it is characterized in that: in the process II, allow ant adopt the mode of roulette method to choose next path point in optional grid around, ant exists tconstantly be positioned at grid ithe time select next grid jtransition probability be:

Figure 2013104170083100001DEST_PATH_IMAGE003

In formula: pfor selecting next path point probability, τ _ijfor pheromone concentration, η _ijfor grid iand grid jbetween the inverse of distance, αfor the relative importance of pheromones, βfor the relative importance of distance, mfor next step optional grid set.

6. the method for planning path for mobile robot based on improving ant group particle cluster algorithm according to claim 3, it is characterized in that: in the process IV, adopt the inertia principle of optimality to be optimized algorithm, to solve, the route broken line is too much, the too much problem of turnover number of times; Suppose g _i (x _i , y _i )for the current path point, g _i-1 (x _i-1 , y _i-1 )for a upper path point, g _i+1 (x _i+1 , y _i+1 )for next path point is g _ione of 8 path points to be chosen on every side, g _j (x _j , y _j )for impact point, concrete formula is as follows:

In formula, s is two spacings between the point of path, cfor constant, in formula the inertia point refer to g _i, g _i-1on same straight line g _i+1, need meet formula:

Figure 2013104170083100001DEST_PATH_IMAGE005

If g _i+1also belong to the inertia point, so for calculating s's (x ' _i+1 , y ' _i+1 )need to be adjusted into:

。