CN104020769B

CN104020769B - Robot overall path planning method based on charge system search

Info

Publication number: CN104020769B
Application number: CN201410264165.XA
Authority: CN
Inventors: 管凤旭; 刘晓龙; 廉德源; 赵拓; 杨长青
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2014-06-13
Filing date: 2014-06-13
Publication date: 2017-02-08
Anticipated expiration: 2034-06-13
Also published as: CN104020769A

Abstract

The invention relates to a robot global path planning method based on charge system search, which establishes a robot path planning mathematical model; initializes the environmental parameters that the robot needs to perform path planning and the relevant parameters of the charge system search algorithm; randomly initializes N paths and each charge Initial position and speed; according to the robot environment information and the robot path planning mathematical model, calculate the fitness value fit of each charge, the best fitness value fitbest, and the worst fitness value fitworst; update the charge quantity q _j of each charge, two The attraction sign p _ij between two charges and the Euclidean distance r _ij between two charges; update the position and velocity of each charge; then recalculate the fitness of each charge according to the robot path planning mathematical model to find out the current The position X _best,new of the charge with the best fitness is the optimal path path of the robot; if the number of iterations is greater than the maximum number of iterations iter _max , exit the loop and output the optimal path path, otherwise return to the next iteration.

Description

A global path planning method for robots based on charge system search

技术领域technical field

本发明涉及一种基于电荷系统搜索的机器人全局路径规划方法。The invention relates to a robot global path planning method based on charge system search.

背景技术Background technique

机器人路径规划是指在其工作空间中,为机器人完成某一给定任务提供一条安全、高效的运动路径。一般而言,机器人完成给定任务可选择的路径有许多条,实际应用中往往要选择一条在一定准则下为最优(或近似最优)的路径,常用的准则有：路径最短、消耗能量最少或使用时间最短等。因此，机器人路径规划实质上是一个有约束的优化问题。Robot path planning refers to providing a safe and efficient motion path for a robot to complete a given task in its workspace. Generally speaking, there are many paths that a robot can choose to complete a given task. In practical applications, it is often necessary to choose a path that is optimal (or nearly optimal) under certain criteria. The commonly used criteria are: the shortest path, the energy consumption Least or shortest usage time etc. Therefore, robot path planning is essentially a constrained optimization problem.

路径规划问题是移动机器人研究领域的一个重要的课题，根据事先掌握环境信息的程度，路径规划问题可分为基于先验环境信息的全局路径规划和基于传感器的局部路径规划。传统的全局路径规划方法主要有栅格法、可视图法、拓扑法和自由空间法等,这些传统的方法均存在计算效率低,不适用于高维优化问题的缺点。随着许多智能仿生优化算法的提出和快速发展，许多学者将这些算法应用于机器人路径规划领域，取得了一定的效果。Path planning is an important topic in the field of mobile robot research. According to the degree of prior knowledge of environmental information, path planning can be divided into global path planning based on prior environmental information and local path planning based on sensors. Traditional global path planning methods mainly include grid method, visualization method, topology method and free space method, etc. These traditional methods have the disadvantages of low computational efficiency and are not suitable for high-dimensional optimization problems. With the proposal and rapid development of many intelligent bionic optimization algorithms, many scholars have applied these algorithms to the field of robot path planning and achieved certain results.

电荷系统搜索算法(Charged System Search，CCS)是一种源自对物理学中的库伦定律和牛顿运动定律进行模拟的新的优化搜索技术，是一种元启发式算法，它通过群体中各个带电电荷的电场力的相互作用产生的群体智能指导优化搜索。Charged System Search algorithm (Charged System Search, CCS) is a new optimization search technology derived from the simulation of Coulomb's law and Newton's law of motion in physics. The swarm intelligence generated by the interaction of the electric force of the charge guides the optimization search.

发明内容Contents of the invention

本发明目的在于提供一种基于电荷系统搜索的机器人全局路径规划方法，计算效率高，能够有效解决复杂动态环境下机器人路径规划问题。The purpose of the present invention is to provide a robot global path planning method based on charge system search, which has high calculation efficiency and can effectively solve the problem of robot path planning in a complex dynamic environment.

实现本发明目的技术方案：Realize the technical scheme of the object of the present invention:

一种基于电荷系统搜索的机器人全局路径规划方法，其特征在于:A robot global path planning method based on charge system search, characterized in that:

步骤1：建立机器人路径规划数学模型；Step 1: Establish a mathematical model for robot path planning;

步骤2：初始化机器人需进行路径规划的环境参数以及电荷系统搜索算法的相关参数；Step 2: Initialize the environmental parameters that the robot needs to perform path planning and the relevant parameters of the charge system search algorithm;

步骤3：随机初始化N条路径以及各个电荷的初始位置和速度，以机器人起始位置和目标位置为x轴建立直角坐标系，将x轴D等分，Step 3: Randomly initialize N paths and the initial position and velocity of each charge, establish a Cartesian coordinate system with the robot's starting position and target position as the x-axis, divide the x-axis D into equal parts,

定义第j个电荷的位置为for j＝1,2,...,N，其中表示第j个电荷在第d维上的位置，每个电荷的位置X_j就代表一条机器人的运行路径，最终所有电荷中的最优位置即为机器人的最优路径；Define the position of the jth charge as for j＝1,2,...,N, where Indicates the position of the j-th charge on the d-dimension, and the position X _j of each charge represents the running path of a robot, and finally the optimal position of all charges is the optimal path of the robot;

步骤4：根据机器人环境信息以及在步骤1中建立的机器人路径规划数学模型，计算各个电荷的适应度值fit,适应度最好值fitbest，适应度最坏值fitworst；Step 4: According to the robot environment information and the robot path planning mathematical model established in step 1, calculate the fitness value fit of each charge, the best fitness value fitbest, and the worst fitness value fitworst;

步骤5：根据步骤4中获得的各个电荷的适应度值fit、适应度最好值fitbest、适应度最坏值fitworst，更新每个电荷的电荷量q_j，两个电荷之间的吸引标志p_ij及两个电荷之间的欧氏距离r_ij；Step 5: According to the fitness value fit, the best fitness value fitbest, and the worst fitness value fitworst of each charge obtained in step 4, update the charge quantity q _j of each charge, and the attraction sign p between two charges _ij and the Euclidean distance r _ij between the two charges;

步骤6：根据步骤5中获得的每个电荷的电荷量q_j，两个电荷之间的吸引标志p_ij及两个电荷之间的欧氏距离r_ij，更新每个电荷的位置及速度；然后根据步骤1中建立的机器人路径规划数学模型重新计算每个电荷的适应度，找出当前适应度最好的电荷的位置X_best,new，即机器人的最优路径path；Step 6: According to the charge amount q _j of each charge obtained in step 5, the attraction sign p _ij between the two charges and the Euclidean distance r _ij between the two charges, update the position and velocity of each charge; Then recalculate the fitness of each charge according to the mathematical model of robot path planning established in step 1, and find out the position X _best,new of the charge with the best current fitness, which is the optimal path path of the robot;

步骤7：若迭代次数大于最大迭代次数iter_max，则退出循环，输出最优路径path，否则返回步骤5进入下一迭代。Step 7: If the number of iterations is greater than the maximum number of iterations iter _max , exit the loop and output the optimal path path, otherwise return to step 5 and enter the next iteration.

步骤1中，通过如下公式，建立机器人路径规划数学模型，In step 1, the mathematical model of robot path planning is established through the following formula,

$F f = = m m i i n no {&Integral; &Integral;}_{00}^{L L} [[α α B B + + ((11 - - α α)) E E.]] d d l l$

式中，F表示广义代价函数，L表示路径长度，B表示环境障碍物对机器人的阻碍代价，E表示机器人运行消耗的能量，α∈[0,1]表示机器人安全运行和消耗能量之间的权衡系数；In the formula, F represents the generalized cost function, L represents the path length, B represents the obstruction cost of the environmental obstacles to the robot, E represents the energy consumed by the robot’s operation, and α∈[0,1] represents the relationship between the robot’s safe operation and energy consumption trade-off factor;

环境障碍物对机器人的阻碍代价B通过如下公式获得，The hindrance cost B of environmental obstacles to the robot is obtained by the following formula,

$B B = = {Σ Σ}_{k k = = 11}^{{N N}_{B B}} \frac{11}{{d d}_{k k}}$

式中，N_B表示障碍物的个数，d_k表示两个节点的中点距第k个障碍物的距离。In the formula, N _B represents the number of obstacles, and d _k represents the distance between the midpoint of two nodes and the kth obstacle.

步骤2中，初始化的参数包括：电荷种群规模N，优化维数D，算法最大迭代次数iter_max，机器人安全运行和消耗能量之间的权衡系数α，环境中的障碍物个数N_B，电荷位置更新方程中的电荷半径a，机器人运行的起点坐标和终点坐标。In step 2, the initialized parameters include: charge population size N, optimization dimension D, algorithm maximum iteration number iter _max , trade-off coefficient α between robot safe operation and energy consumption, number of obstacles in the environment N _B , charge The charge radius a in the position update equation, the coordinates of the start point and end point of the robot operation.

步骤5中，更新公式如下，In step 5, the update formula is as follows,

${q q}_{j j} = = \frac{f f i i t t ((j j)) - - f f i i t t w w o o r r s the s t t}{f f i i t t b b e e s the s t t - - f f i i t t w w o o r r s the s t t},, j j = = 11,, 22,, ... ...,, N N$

${r r}_{i i j j} = = \frac{| | | | {X x}_{i i} - - {X x}_{j j} | | | |}{| | | | (({X x}_{i i} + + {X x}_{j j})) / / 22 - - {X x}_{b b e e s the s t t} | | | | + + ϵ ϵ}$

式中，X_i和X_j分别是第i个电荷和第j个电荷在空间的位置，X_best是目前所有电荷的最好位置，即fitbest对应的电荷的位置，ε是一个很小的正常数，以防止除数为零。In the formula, X _i and X _j are the positions of the i-th charge and the j-th charge in space respectively, X _best is the best position of all charges at present, that is, the position of the charge corresponding to fitbest, and ε is a small normal number to prevent division by zero.

步骤6中，每个电荷的位置及速度更新公式如下，In step 6, the position and velocity update formula of each charge is as follows,

$\begin{matrix} {X x}_{j j,, n no e e w w} = = 0.5 0.5 {rand rand}_{j j 11} \cdot &Center Dot; ((11 + + i i t t e e r r / / {iter iter}_{max max})) \underset{i i,, i i &NotEqual; &NotEqual; j j}{Σ Σ} ((\frac{{q q}_{i i}}{{a a}^{33}} {r r}_{i i j j} {i i}_{11} + + \frac{{q q}_{i i}}{{r r}_{i i j j}^{22}} {i i}_{22})) {p p}_{i i j j} (({X x}_{i i,, o o l l d d} - - {X x}_{j j,, o o l l d d})) \\ + + 0.5 0.5 {rand rand}_{j j 22} \cdot &Center Dot; ((11 - - i i t t e e r r / / {iter iter}_{max max})) {V V}_{j j,, o o l l d d} + + {X x}_{j j,, o o l l d d} \end{matrix}$

V_j,new＝X_j,new-X_j,old。V _j,new =X _j,new -X _j,old .

本发明具有的有益效果：The beneficial effect that the present invention has:

本发明提出了一种电荷系统搜索算法模型，并将其成功应用于解决复杂环境下的机器人路径规划问题。不同于其它仿生智能算法，电荷系统搜索过程中所体现出的并行性、协同性、自组织性、动态性、强鲁棒性等特点与复杂的机器人运行环境的十分相符，因此电荷系统搜索算法可有效解决机器人复杂动态环境下的路径规划问题。The invention proposes a charge system search algorithm model, which is successfully applied to solve the problem of robot path planning in complex environments. Different from other bionic intelligent algorithms, the characteristics of parallelism, collaboration, self-organization, dynamics, and strong robustness reflected in the charge system search process are very consistent with the complex robot operating environment, so the charge system search algorithm It can effectively solve the path planning problem in the complex dynamic environment of the robot.

本发明计算效率高，具有很好的实时性和快速性，所搜索到的路径更逼近实际的机器人最优路径，使机器人能够在满足安全性能指标和能量消耗指标的前提下到达目标位置，是解决复杂动态环境下机器人路径规划的有效技术途径。本发明不仅可以应用于机器人路径规划，也可应用于复杂环境下的无人机航路规划、水下机器人路径规划等技术领域。本发明为高维函数优化问题提供了一条非常有效的途径，可广泛应用于机器人、航空、航天、航海、工业生产等涉及多维函数优化问题的领域。The invention has high calculation efficiency, good real-time performance and rapidity, and the searched path is closer to the actual optimal path of the robot, so that the robot can reach the target position under the premise of satisfying the safety performance index and the energy consumption index, which is beneficial An effective technical approach to solve robot path planning in complex dynamic environments. The present invention can be applied not only to path planning of robots, but also to technical fields such as path planning of unmanned aerial vehicles and path planning of underwater robots under complex environments. The invention provides a very effective way for the optimization problem of high-dimensional function, and can be widely used in the fields involving multi-dimensional function optimization problem such as robot, aviation, spaceflight, navigation, industrial production and the like.

附图说明Description of drawings

图1机器人系统原理框图；Fig. 1 Principle block diagram of the robot system;

图2机器人路径规划示意图；Fig. 2 schematic diagram of robot path planning;

图3环境障碍物对机器人阻碍代价计算示意图；Fig. 3 Schematic diagram of the calculation of environmental obstacles to the obstacle cost of the robot;

图4电荷之间相互作用示意图；Figure 4 Schematic diagram of the interaction between charges;

图5本发明基于电荷系统搜索的机器人全局路径规划方法程序流程图；Fig. 5 program flowchart of the robot global path planning method based on charge system search in the present invention;

图6本发明方法得到的机器人路径规划最优结果示意图；Fig. 6 is a schematic diagram of the optimal result of robot path planning obtained by the method of the present invention;

图中标号及符号说明如下：The labels and symbols in the figure are explained as follows:

图3中：k—第k个障碍物，k-1—第k-1个障碍物，k+1—第k+1个障碍物，k+2—第k+2个障碍物；(x_i-1,y_i-1)—机器人路径中第i-1个节点的坐标，(x_i,y_i)—机器人路径中第i个节点的坐标；d_k—机器人路径中第i-1个节点与第i个节点的中点距第k个障碍物的距离，d_k-1—机器人路径中第i-1个节点与第i个节点的中点距第k-1个障碍物的距离，d_k+1—机器人路径中第i-1个节点与第i个节点的中点距第k+1个障碍物的距离，d_k+2—机器人路径中第i-1个节点与第i个节点的中点距第k+2个障碍物的距离；In Figure 3: k—kth obstacle, k-1—k-1th obstacle, k+1—k+1th obstacle, k+2—k+2th obstacle; (x _i-1 ,y _i-1 )—the coordinates of the i-1th node in the robot path, ( _xi ,y _i )—the coordinates of the i-th node in the robot path; d _k —the i-1th node in the robot path The distance between the midpoint of the i-th node and the i-th node to the k-th obstacle, d _k-1 — the distance between the i-1th node and the i-th node’s midpoint in the robot’s path to the k-1th obstacle Distance, d _k+1 — the distance between the i-1th node and the i-th node in the robot path to the k+1th obstacle, d _k+2 — the distance between the i-1th node and the i-th node in the robot path The distance between the midpoint of the i-th node and the k+2th obstacle;

图4中：q₁—第1个电荷的电荷量，q₂—第2个电荷的电荷量，q₃—第3个电荷的电荷量，q₄—第4个电荷的电荷量，q₅—第5个电荷的电荷量，q₆—第6个电荷的电荷量；F₁₄—电荷q₁对电荷q₄的电场力，F₂₄—电荷q₂对电荷q₄的电场力，F₃₄—电荷q₃对电荷q₄的电场力，F₆₄—电荷q₆对电荷q₄的电场力，F₄—电荷q₁、q₂、q₃、q₆对电荷q₄的电场力的合力；In Figure 4: q ₁ - the amount of charge of the first charge, q ₂ - the amount of charge of the second charge, q ₃ - the amount of charge of the third charge, q ₄ - the amount of charge of the fourth charge, q ₅ —the charge quantity of the fifth charge, q ₆ —the charge quantity of the sixth charge; F ₁₄ —the electric field force of the charge q ₁ on the charge q ₄ , F ₂₄ —the electric field force of the charge q ₂ on the charge q ₄ , F ₃₄ —the electric field force of charge q ₃ on charge q ₄ , F ₆₄ —the electric field force of charge q ₆ on charge q ₄ , F ₄ —the resultant force of the electric field force of charges q ₁ , q ₂ , q ₃ , q ₆ on charge q ₄ ;

图5中：N—种群中电荷的个数，iter—算法当前的迭代次数，iter_max—算法的最大迭代次数，path—机器人的最优路径，j—第j个电荷。In Figure 5: N—the number of charges in the population, iter—the current iteration number of the algorithm, iter _max —the maximum iteration number of the algorithm, path—the optimal path of the robot, j—the jth charge.

具体实施方式detailed description

如图1所示，机器人系统原理为，机器人核心控制单元负责机器人的运动控制和各个传感器信息的采集及处理；机器人伺服电机驱动系统接收核心控制单元的控制信号驱动机器人电机运动；机器人电量测量传感器能够测量机器人当前的电量，设机器人在初始位置时的电量为E_o，到达目标位置时的电量为E_f，则机器人运行过程消耗的能量E＝E_o-E_f；机器人避障传感器能够实时测量机器人前方障碍物的距离信息；通过机器人光电码盘传感器能够测量机器人的运行速度，结合机器人运行的时间即可求出机器人运行的路程信息。As shown in Figure 1, the principle of the robot system is that the core control unit of the robot is responsible for the motion control of the robot and the collection and processing of information from various sensors; the servo motor drive system of the robot receives the control signal from the core control unit to drive the motor of the robot; the power measurement sensor of the robot It can measure the current power of the robot. Let the power when the robot is at the initial position be E _o , and the power when it reaches the target position is E _f , then the energy consumed by the robot during operation is E=E _o -E _f ; the obstacle avoidance sensor of the robot can real-time Measure the distance information of the obstacle in front of the robot; the robot's running speed can be measured through the robot's photoelectric code disc sensor, and the distance information of the robot's running can be obtained by combining the running time of the robot.

参考图2，以机器人的起始位置和目标位置连线为x轴并将其D等分，机器人的每条路径均可用X＝(x¹,x²,...,x^d,...,x^D)表示，其中x^d表示第d个等分点的y坐标值，D值越大，机器人路径规划越精细。可以看出图中的虚线路径明显优于其他两条路径，机器人路径规划的目的就是在满足一定的条件下，找到一条最优的路径X^*，从而将一个机器人路径规划问题转化为一个D维的函数优化问题。Referring to Figure 2, take the line connecting the starting position and the target position of the robot as the x-axis and divide it equally into D, each path of the robot can be used X=(x ¹ ,x ² ,...,x ^d ,.. .,x ^D ), where x ^d represents the y-coordinate value of the d-th bisection point, the larger the value of D, the finer the robot path planning. It can be seen that the dotted line path in the figure is obviously better than the other two paths. The purpose of robot path planning is to find an optimal path X ^* under certain conditions, so as to transform a robot path planning problem into a D-dimensional function optimization problem.

机器人路径规划是利用一种确定性状态空间搜索方法，减小规划空间的规模。将机器人路径规划问题简化成为一个二维路径规划问题，即一个D维函数优化问题，然后根据机器人路径规划目标，建立其数学模型如下：Robot path planning uses a deterministic state space search method to reduce the size of the planning space. The robot path planning problem is simplified into a two-dimensional path planning problem, that is, a D-dimensional function optimization problem, and then according to the robot path planning goal, its mathematical model is established as follows:

$F f = = min min {&Integral; &Integral;}_{00}^{L L} [[α α B B + + ((11 - - α α)) E E.]] d d l l$

其中F表示广义代价函数，L表示路径长度，B表示环境障碍物对机器人的阻碍代价，E表示机器人运行消耗的能量，α∈[0,1]表示机器人安全运行和消耗能量之间的权衡系数，如果机器人运行的安全性很重要，则α选择较大的值；如果机器人到达目标的快速性很重要，则α选择较小的值。此处机器人路径规划目的就是在最小化广义代价函数的前提下，为机器人计算一条最优(或近似最优)的路径。Among them, F represents the generalized cost function, L represents the path length, B represents the obstruction cost of environmental obstacles to the robot, E represents the energy consumed by the robot’s operation, and α∈[0,1] represents the trade-off coefficient between the robot’s safe operation and energy consumption , if the safety of the robot's operation is very important, then α chooses a larger value; if the speed of the robot's arrival at the target is very important, then α chooses a smaller value. The purpose of robot path planning here is to calculate an optimal (or nearly optimal) path for the robot on the premise of minimizing the generalized cost function.

机器人运行消耗的能量E与其路径的长度有关，环境障碍物对机器人的阻碍代价B计算如下，The energy E consumed by the robot’s operation is related to the length of its path, and the obstruction cost B of the environmental obstacles to the robot is calculated as follows,

$B B = = {Σ Σ}_{k k = = 11}^{{N N}_{B B}} \frac{11}{{d d}_{k k}}$

其中N_B表示障碍物的个数，参考图3，d_k表示两个节点的中点距第k个障碍物的距离。Where N _B represents the number of obstacles, referring to Figure 3, d _k represents the distance between the midpoint of two nodes and the kth obstacle.

电荷系统搜索算法(Charged System Search，CCS)是一种源自对物理学中的库伦定律和牛顿运动定律进行模拟的新的优化搜索技术，它通过群体中各个带电电荷的电场力的相互作用产生的群体智能指导优化搜索。参考图4，每个电荷会在其周围产生电场并作用于其他电荷，图中电荷q₄受到了来自电荷q₁,q₂,q₃,q₅和q₆的作用力最终沿合力F₄的方向运动。每个电荷在其他电荷作用力下在搜索空间中运动，其位置更新公式及速度更新公式如下：The charge system search algorithm (Charged System Search, CCS) is a new optimization search technology derived from the simulation of Coulomb's law and Newton's law of motion in physics, which is generated by the interaction of the electric field force of each charged charge in the group The intelligence of swarms guides optimized searches. Referring to Figure 4, each charge will generate an electric field around it and act on other charges. In the figure, charge q ₄ is subjected to forces from charges q ₁ , q ₂ , q ₃ , q ₅ and q ₆ and finally along the resultant force F ₄ direction of movement. Each charge moves in the search space under the force of other charges, and its position update formula and velocity update formula are as follows:

V_j,new＝X_j,new-X_j,old V _j,new ＝X _j,new -X _j,old

其中X_j,new表示第j个电荷的新位置，X_j,old表示第j个电荷的旧位置，X_i,old表示第i个电荷的旧位置，V_j,new表示第j个电荷的新速度，V_j,old表示第j个电荷的旧速度，q_i表示第i个电荷的电荷量，r_ij表示第i个电荷和第j个电荷在空间的欧氏距离，rand_j1和rand_j2是均匀分布在(0,1)的随机数，iter表示算法当前的迭代次数，iter_max表示算法的最大迭代次数，a表示将电荷视为一个带电球体的半径，i₁和i₂定义如下：Where X _{j, new} represents the new position of the jth charge, X _{j, old} represents the old position of the jth charge, X _{i, old} represents the old position of the i charge, V _{j, new} represents the j charge New speed, V _j,old represents the old speed of the jth charge, q _i represents the charge amount of the i-th charge, r _ij represents the Euclidean distance between the i-th charge and the j-th charge in space, rand _j1 and rand _j2 is a random number evenly distributed in (0,1), iter represents the current iteration number of the algorithm, iter _max represents the maximum iteration number of the algorithm, a represents the radius of the charge as a charged sphere, and i ₁ and i ₂ are defined as follows :

$\{\begin{matrix} {i i}_{11} = = 11,, {i i}_{22} = = 00 &DoubleLeftRightArrow; &DoubleLeftRightArrow; {r r}_{i i j j} < < a a \\ {i i}_{11} = = 00,, {i i}_{22} = = 11 &DoubleLeftRightArrow; &DoubleLeftRightArrow; {r r}_{i i j j} &GreaterEqual; &Greater Equal; a a \end{matrix}$

p_ij表示第j个电荷是否受到第i个电荷的吸引力，定义如下：p _ij indicates whether the j-th charge is attracted by the i-th charge, defined as follows:

其中fit(i)表示第i个电荷当前的适应度，fit(j)表示第j个电荷当前的适应度，fitbest表示目前所有电荷最好的适应度，rand是均匀分布在(0,1)的随机数。Among them, fit(i) represents the current fitness of the i-th charge, fit(j) represents the current fitness of the j-th charge, fitbest represents the best fitness of all charges at present, and rand is uniformly distributed in (0,1) of random numbers.

下面通过一个具体实例，对本发明所提出的基于电荷系统搜索的机器人全局路径规划方法进一步详细说明。实验环境为2.70Ghz，2G内存，MATLAB R2012b版本。The method for global path planning of a robot based on charge system search proposed by the present invention will be further described in detail through a specific example below. The experimental environment is 2.70Ghz, 2G memory, MATLAB R2012b version.

参考图5，一种基于电荷系统搜索的机器人全局路径规划方法，其具体实现Referring to Figure 5, a global path planning method for a robot based on charge system search, its specific implementation

步骤如下：步骤1：机器人路径规划数学模型的建立:The steps are as follows: Step 1: the establishment of the robot path planning mathematical model:

机器人环境数学模型的建立：The establishment of the mathematical model of the robot environment:

利用一种确定性状态空间搜索方法，减小规划空间的规模，将机器人路径规划问题简化成为一个二维路径规划问题，即一个D维函数优化问题。A deterministic state space search method is used to reduce the scale of the planning space, and the robot path planning problem is simplified into a two-dimensional path planning problem, that is, a D-dimensional function optimization problem.

$F f = = m m i i n no {&Integral; &Integral;}_{00}^{L L} [[α α B B + + ((11 - - α α)) E E.]] d d l l - - - - - - ((11))$

其中F表示广义代价函数，L表示路径长度，B表示环境障碍物对机器人的阻碍代价，E表示机器人运行消耗的能量，α∈[0,1]表示机器人安全运行和消耗能量之间的权衡系数，如果机器人运行的安全性很重要，则α选择较大的值；如果机器人到达目标的快速性很重要，则α选择较小的值。Among them, F represents the generalized cost function, L represents the path length, B represents the obstruction cost of environmental obstacles to the robot, E represents the energy consumed by the robot’s operation, and α∈[0,1] represents the trade-off coefficient between the robot’s safe operation and energy consumption , if the safety of the robot's operation is very important, then α chooses a larger value; if the speed of the robot's arrival at the target is very important, then α chooses a smaller value.

机器人路径规划优化性能指标数学模型的建立：The establishment of the mathematical model of the robot path planning optimization performance index:

根据机器人执行任务的安全性能指标和能量消耗性能指标，对机器人环境障碍物对机器人的阻碍代价建立数学模型。According to the safety performance index and energy consumption performance index of the robot's task execution, a mathematical model is established for the obstruction cost of the robot's environmental obstacles to the robot.

$B B = = {Σ Σ}_{k k = = 11}^{{N N}_{B B}} \frac{11}{{d d}_{k k}} - - - - - - ((22))$

步骤2：初始化机器人需进行路径规划的环境参数以及电荷系统搜索算法的相关参数。Step 2: Initialize the environmental parameters that the robot needs to perform path planning and the relevant parameters of the charge system search algorithm.

参数设置为：电荷种群规模为N＝20，优化维数为D＝30，算法最大迭代次数为iter_max＝200，机器人安全运行和消耗能量之间的权衡系数α＝0.5，环境中的障碍物个数N_B＝5，位置更新方程中的电荷半径a＝1，机器人运行的起点坐标[0,0]和终点坐标[60,110]。The parameters are set as follows: the charge population size is N=20, the optimization dimension is D=30, the maximum number of iterations of the algorithm is iter _max =200, the trade-off coefficient between the robot’s safe operation and energy consumption is α=0.5, and the obstacles in the environment The number N _B =5, the charge radius a=1 in the position update equation, the starting point coordinates [0,0] and the end point coordinates [60,110] of the robot operation.

步骤3：随机初始化N条路径以及各个电荷的初始位置X_j，for j＝1,2,...,N和初始速度for j＝1,2,...,N，以机器人起始位置和目标位置为x轴建立直角坐标系，将x轴D等分。Step 3: Randomly initialize N paths and the initial position X _j of each charge, for j=1,2,...,N and initial velocity For j=1,2,...,N, establish a Cartesian coordinate system with the robot's starting position and target position as the x-axis, and divide the x-axis D into equal parts.

步骤4：根据机器人环境信息以及在步骤1中建立的机器人路径规划数学模型，计算每一条路径中环境障碍物对机器人的阻碍代价，得出各个电荷的适应度值fit,适应度最好值fitbest，适应度最坏值fitworst。Step 4: According to the robot environment information and the robot path planning mathematical model established in step 1, calculate the obstacle cost of the environmental obstacles to the robot in each path, and obtain the fitness value fit of each charge, and the best fitness value fitbest , the worst value of fitness fitworst.

步骤5：更新每个电荷的电荷量q_j，两个电荷之间的吸引标志p_ij及两个电荷之间的欧氏距离r_ij，其更新公式分别如下：Step 5: Update the charge quantity q _j of each charge, the attraction sign p _ij between the two charges and the Euclidean distance r _ij between the two charges, and the update formulas are as follows:

${q q}_{j j} = = \frac{f f i i t t ((j j)) - - f f i i t t w w o o r r s the s t t}{f f i i t t b b e e s the s t t - - f f i i t t w w o o r r s the s t t},, j j = = 11,, 22,, ... ...,, N N - - - - - - ((33))$

${r r}_{i i j j} = = \frac{| | | | {X x}_{i i} - - {X x}_{j j} | | | |}{| | | | (({X x}_{i i} + + {X x}_{j j})) / / 22 - - {X x}_{b b e e s the s t t} | | | | + + ϵ ϵ} - - - - - - ((55))$

其中X_i和X_j分别是第i个电荷和第j个电荷在空间的位置，X_best是目前所有电荷的最好位置，即fitbest对应的电荷的位置，ε是一个很小的正常数，以防止除数为零。Among them, X _i and X _j are the positions of the i-th charge and the j-th charge in space respectively, X _best is the best position of all charges at present, that is, the position of the charge corresponding to fitbest, ε is a small constant, to prevent division by zero.

步骤6：根据如下公式更新每个电荷的位置及速度：Step 6: Update the position and velocity of each charge according to the following formula:

$\begin{matrix} {X x}_{j j,, n no e e w w} = = 0.5 0.5 {rand rand}_{j j 11} \cdot &Center Dot; ((11 + + i i t t e e r r / / {iter iter}_{max max})) \underset{i i,, i i &NotEqual; &NotEqual; j j}{Σ Σ} ((\frac{{q q}_{i i}}{{a a}^{33}} {r r}_{i i j j} {i i}_{11} + + \frac{{q q}_{i i}}{{r r}_{i i j j}^{22}} {i i}_{22})) {p p}_{i i j j} (({X x}_{i i,, o o l l d d} - - {X x}_{j j,, o o l l d d})) \\ + + 0.5 0.5 {rand rand}_{j j 22} \cdot &Center Dot; ((11 - - i i t t e e r r / / {iter iter}_{max max})) {V V}_{j j,, o o l l d d} + + {X x}_{j j,, o o l l d d} \end{matrix} - - - - - - ((66))$

V_j,new＝X_j,new-X_j,old (7)V _j,new ＝X _j,new -X _j,old (7)

然后根据机器人路径规划的数学模型重新计算评估每个电荷的适应度fit,找出适应度最好值fitbest，最坏值fitworst以及fitbest对应的电荷位置X_best,new。机器人的当前最优路径表示为Then recalculate and evaluate the fitness fit of each charge according to the mathematical model of robot path planning, and find out the best fitness value fitbest, the worst value fitworst and the charge position X _best,new corresponding to fitbest. The current optimal path of the robot is expressed as

其中X_{best,previous}表示所有电荷前一次迭代的最优位置。Where X _{best, previous} represents the optimal position of all charges in the previous iteration.

图6即为实验运行结果，本发明方法为机器人规划出一条可行的、有效的路径，成功地避过了环境中的障碍物并到达目标点。Figure 6 is the result of the experiment. The method of the present invention plans a feasible and effective path for the robot, successfully avoiding obstacles in the environment and reaching the target point.

Claims

1. A robot global path planning method based on charge system search, comprising the following steps:

Step 1: Establish a mathematical model for robot path planning;

Step 2: Initialize the environmental parameters that the robot needs to perform path planning and the relevant parameters of the charge system search algorithm;

Step 3: Randomly initialize N paths and the initial position and velocity of each charge, establish a Cartesian coordinate system with the robot's starting position and target position as the x-axis, divide the x-axis D into equal parts,

Define the position of the jth charge as in Indicates the position of the j-th charge on the d-dimension, and the position X _j of each charge represents the running path of a robot, and finally the optimal position of all charges is the optimal path of the robot;

Step 4: According to the robot environment information and the robot path planning mathematical model established in step 1, calculate the fitness value fit of each charge, the best fitness value fitbest, and the worst fitness value fitworst;

Step 5: According to the fitness value fit, the best fitness value fitbest, and the worst fitness value fitworst of each charge obtained in step 4, update the charge quantity q _j of each charge, and the attraction sign p between two charges _ij and the Euclidean distance r _ij between the two charges;

Step 6: According to the charge amount q _j of each charge obtained in step 5, the attraction sign p _ij between the two charges and the Euclidean distance r _ij between the two charges, update the position and velocity of each charge; Then recalculate the fitness of each charge according to the mathematical model of robot path planning established in step 1, and find out the position X _best,new of the charge with the best current fitness, which is the optimal path path of the robot;

Step 7: If the number of iterations is greater than the maximum number of iterations iter _max , exit the loop and output the optimal path path, otherwise return to step 5 and enter the next iteration;

It is characterized in that: in step 1, a robot path planning mathematical model is established through the following formula,

F f = = min min {&Integral; &Integral;}_{00}^{L L} [[α α B B + + ((11 - - α α)) E E.]] d d l l

In the formula, F represents the generalized cost function, L represents the path length, B represents the obstruction cost of the environmental obstacles to the robot, E represents the energy consumed by the robot’s operation, and α∈[0,1] represents the relationship between the robot’s safe operation and energy consumption trade-off factor;

The hindrance cost B of environmental obstacles to the robot is obtained by the following formula,

B B = = {Σ Σ}_{k k = = 11}^{{N N}_{B B}} \frac{11}{{d d}_{k k}}

In the formula, N _B represents the number of obstacles, and d _k represents the distance between the midpoint of two nodes and the kth obstacle.

2. The robot global path planning method based on charge system search according to claim 1, characterized in that: in step 2, the initialized parameters include: charge population size N, optimization dimension D, algorithm maximum iteration number iter _max , The trade-off coefficient α between the safe operation of the robot and the energy consumption, the number of obstacles N _B in the environment, the charge radius a in the charge position update equation, the starting point coordinates and the end point coordinates of the robot operation.

3. the robot global path planning method based on charge system search according to claim 2, is characterized in that:

In step 5, the update formula is as follows,

{q q}_{j j} = = \frac{f f i i t t ((j j)) - - f f i i t t w w o o r r s the s t t}{f f i i t t b b e e s the s t t - - f f i i t t w w o o r r s the s t t},, j j = = 11,, 22,, ... ...,, N N

{r r}_{i i j j} = = \frac{| | | | {X x}_{i i} - - {X x}_{j j} | | | |}{| | | | (({X x}_{i i} + + {X x}_{j j})) / / 22 - - {X x}_{b b e e s the s t t} | | | | + + ϵ ϵ}

In the formula, X _i and X _j are the positions of the i-th charge and the j-th charge in space respectively, X _best is the best position of all charges at present, that is, the position of the charge corresponding to fitbest, and ε is a small normal number to prevent division by zero.

4. the robot global path planning method based on charge system search according to claim 3, is characterized in that:

In step 6, the position and velocity update formula of each charge is as follows,

\begin{matrix} {X x}_{j j,, n no e e w w} = = 0.5 0.5 {rand rand}_{j j 11} \cdot \cdot ((11 + + i i t t e e r r / / {iter iter}_{max max})) \underset{i i,, i i &NotEqual; &NotEqual; j j}{Σ Σ} ((\frac{{q q}_{i i}}{{a a}^{33}} {r r}_{i i j j} {i i}_{11} + + \frac{{q q}_{i i}}{{r r}_{i i j j}^{22}} {i i}_{22})) {p p}_{i i j j} (({X x}_{i i,, o o l l d d} - - {X x}_{j j,, o o l l d d})) \\ + + 0.5 0.5 {rand rand}_{j j 22} \cdot \cdot ((11 - - i i t t e e r r / / {iter iter}_{max max})) {V V}_{j j,, o o l l d d} + + {X x}_{j j,, o o l l d d} \end{matrix}

V _j,new =X _j,new -X _j,old .