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

Abstract

The invention relates to a robot overall path planning method based on the charge system search. The method includes the steps that a robot path planning mathematic model is established; environment parameters for planning paths of a robot and relevant parameters of a charge system search algorithm are initialized; N paths and the initial position and the speed of each charge are randomly initialized; according to robot environment information and the robot path planning mathematic model, the fitness value fit, the best fitness value fitbest and the worst fitness value fitworst of each charge are calculated; the charge quantity qj of each charge, an attraction mark pij between every two charges and the Euclidean distance rij between every two charges are updated; the position and the speed of each charge are updated; fitness of each charge is recalculated according to the robot path planning mathematic model, the position Xbest new of the charge with the best fitness currently is found, and therefore the optimal path of the robot is obtained; if the number of iteration times is larger than the number itermax of iteration, a circulation is ended, the optimal path is output, and otherwise iteration of the next time is conducted.

Description

A kind of robot global path planning method based on Charge System search

Technical field

The present invention relates to a kind of robot global path planning method based on Charge System search.

Background technology

Robot path planning refers to that, in its work space, completing a certain Given task for robot provides a peace Entirely, efficient motion path.Generally speaking, robot completes the selectable path of Given task has many bars, in practical application Often to select one under certain criterion be optimum (or near-optimization) path, conventional criterion has：Shortest path, consumption Energy is minimum or use time is the shortest etc..Therefore, robot path planning is substantially a constrained optimization problem.

Path planning problem is an important problem of mobile robot research field, grasps environmental information according to prior Degree, path planning problem can be divided into global path planning and sensor-based local path based on priori environment information Planning.Traditional global path planning method mainly has Grid Method, Visual Graph method, topological approach and free-space Method etc., these biographies All there is the shortcoming that computational efficiency is low, is not suitable for high-dimensional optimization in the method for system.With many intelligent bionic optimized algorithms Proposition and fast development, these algorithms are applied to robot path planning field, achieve certain effect by many scholars.

Charge System searching algorithm (Charged System Search, CCS) is a kind of to the coulomb in physicss Law and the new Optimizing Search technology of Newton's laws of motion simulation, are a kind of meta-heuristic algorithms, it passes through in colony The swarm intelligence producing that interacts of the electric field force of each charged particles instructs Optimizing Search.

Content of the invention

Present invention aim at providing a kind of robot global path planning method based on Charge System search, calculate effect Rate is high, can effectively solve the problem that robot path planning's problem under complex dynamic environment.

Realize the object of the invention technical scheme：

A kind of robot global path planning method based on Charge System search it is characterised in that:

Step 1：Set up robot path planning's mathematical model；

Step 2：Initialization robot need to carry out the ambient parameter of path planning and the correlation of Charge System searching algorithm Parameter；

Step 3：The initial position of random initializtion N paths and each electric charge and speed, with robot original position Set up rectangular coordinate system with target location for x-axis, by x-axis D decile,

The position defining j-th electric charge isFor j=1,2 ..., N, whereinRepresent J-th electric charge position in d dimension, the position X of each electric charge_jJust represent the operating path of a robot, finally own Optimal location in electric charge is the optimal path of robot；

Step 4：The robot path planning's mathematical model set up according to robot environment's information and in step 1, meter Calculate the fitness value fit of each electric charge, fitness best values fitbest, fitness bad value fitworst；

Step 5：According to the fitness value fit of each electric charge of acquisition, fitness best values fitbest, adaptation in step 4 Degree bad value fitworst, updates the quantity of electric charge q of each electric charge_j, attraction mark p between two electric charges_ijAnd two electric charges it Between Euclidean distance r_ij；

Step 6：Quantity of electric charge q according to each electric charge obtaining in step 5_j, attraction mark p between two electric charges_ijAnd Euclidean distance r between two electric charges_ij, update position and the speed of each electric charge；Then according to the robot setting up in step 1 Path planning mathematical model recalculates the fitness of each electric charge, finds out the position of the best electric charge of current fitness X_best,new, i.e. the optimal path path of robot；

Step 7：If iterationses are more than maximum iteration time iter_max, then exit circulation, export optimal path path, no Then return to step 5 enters following iteration.

In step 1, by equation below, set up robot path planning's mathematical model,

$F=min{\∫}_0^L\[\mathrm{\α}B+(1-\mathrm{\α})E\]dl$

In formula, F represents generalized cost function, and L represents path, and B represents the obstruction generation to robot for the Environment Obstacles thing Valency, E represents that robot runs the energy consuming, and α ∈ [0,1] represents that robot security runs the balance system and consumed energy between Number；

Environment Obstacles thing is obtained by equation below to obstruction cost B of robot,

$B=\underset{k=1}{\overset{N_B}{\Σ}}\frac{1}{d_k}$

In formula, N_BRepresent the number of barrier, d_kThe distance away from k-th barrier for the midpoint of two nodes of expression.

In step 2, initialized parameter includes：Electric charge population scale N, optimizes dimension D, algorithm maximum iteration time iter_max, robot security run and consumed energy between balance factor alpha, barrier number N in environment_B, Charge sites Charge radius a in renewal equation, starting point coordinate and terminal point coordinate that robot runs.

In step 5, more new formula is as follows,

$q_j=\frac{fit\left(j\right)-fitworst}{fitbest-fitworst},j=1,2,...,N$

$r_{ij}=\frac{\left|\right|X_i-X_j\left|\right|}{\left|\right|(X_i+X_j)/2-X_{best}\left|\right|+\mathrm{\ϵ}}$

In formula, X_iAnd X_jIt is i-th electric charge and j-th electric charge respectively in the position in space, X_bestIt is all electric charges at present Desired positions, i.e. the position of the corresponding electric charge of fitbest, ε is the normal number of a very little, to prevent divisor from being zero.

In step 6, the position of each electric charge and speed more new formula are as follows,

$\begin{array}{c}{X}_{j,new}=0.5{\mathrm{rand}}_{j1}\·(1+iter/{\mathrm{iter}}_{\max })\underset{i,i\≠j}{\Σ}\left(\frac{q_i}{a^3}{r}_{ij}{i}_1+\frac{q_i}{{r_{ij}}^2}{i}_2\right)p_{ij}(X_{i,old}-X_{j,old})\\ +0.5{\mathrm{rand}}_{j2}\·(1-iter/{\mathrm{iter}}_{\max })V_{j,old}+X_{j,old}\end{array}$

V_j,new=X_j,new-X_j,old.

The device have the advantages that：

The present invention proposes a kind of Charge System searching algorithm model, and is successfully applied to solve under complex environment Robot path planning's problem.Different from other bionic intelligence algorithms, the concurrency that goes out embodied in Charge System search procedure, The features such as concertedness, self-organization, dynamic, strong robustness, is extremely consistent with complicated robot running environment, therefore electricity G system searching algorithm can path planning problem under effectively solving robot complex dynamic environment.

Computational efficiency of the present invention is high, has good real-time and rapidity, and reality is more approached in the path being searched Robot optimal path, allows the robot to reach target position on the premise of meeting safe performance indexes and energy expenditure index Put, be the effective technical way solving robot path planning under complex dynamic environment.The present invention can be applied not only to machine People's path planning, also apply be applicable to the technical fields such as Path Planning for Unmanned Aircraft Vehicle under complex environment, underwater robot path planning. The present invention provides a very effective approach for high-dimensional function optimization problem, can be widely applied to robot, aviation, boat My god, navigation, commercial production etc. be related to the field of multidimensional function optimization problem.

Brief description

Fig. 1 robot system theory diagram；

Fig. 2 robot path planning's schematic diagram；

Fig. 3 Environment Obstacles thing hinders cost to calculate schematic diagram in robot；

The interphase interaction schematic diagram of Fig. 4 electric charge；

The robot global path planning method program flow diagram that Fig. 5 present invention is searched for based on Charge System；

Robot path planning's optimal result schematic diagram that Fig. 6 the inventive method obtains；

In figure label and symbol description are as follows：

In Fig. 3：K-th barrier of k, -1 barrier of k-1 kth ,+1 barrier of k+1 kth, k+2 kth+2 Individual barrier；(x_i-1,y_i-1) coordinate of the i-th -1 node, (x in robot path_i,y_i) i-th section in robot path The coordinate of point；d_kThe distance away from k-th barrier for the midpoint of the i-th -1 node and i-th node in robot path, d_k-1The distance away from -1 barrier of kth for the midpoint of the i-th -1 node and i-th node, d in robot path_k+1Machine The distance away from+1 barrier of kth for the midpoint of the i-th -1 node and i-th node, d in people path_k+2In robot path The distance away from+2 barriers of kth for the midpoint of i-1 node and i-th node；

In Fig. 4：q₁The quantity of electric charge of the 1st electric charge, q₂The quantity of electric charge of the 2nd electric charge, q₃The electric charge of the 3rd electric charge Amount, q₄The quantity of electric charge of the 4th electric charge, q₅The quantity of electric charge of the 5th electric charge, q₆The quantity of electric charge of the 6th electric charge；F₁₄Electric charge q₁To electric charge q₄Electric field force, F₂₄Electric charge q₂To electric charge q₄Electric field force, F₃₄Electric charge q₃To electric charge q₄Electric field force, F₆₄— Electric charge q₆To electric charge q₄Electric field force, F₄Electric charge q₁、q₂、q₃、q₆To electric charge q₄Electric field force make a concerted effort；

In Fig. 5：The number of electric charge in N population, the current iterationses of iter algorithm, iter_maxThe maximum of algorithm Iterationses, the optimal path of path robot, j-th electric charge of j.

Specific embodiment

As shown in figure 1, robot system principle is, robot core control unit is responsible for the motor control of robot and each The collection of individual sensor information and process；The control signal that robot servo motors' drive system receives key control unit drives Robot motor moves；Robot electric quantity measurement sensor can the current electricity of robot measurement, if robot is in initial bit Electricity when putting is E_o, reaching electricity during target location is E_f, then ENERGY E=E that robot running consumes_o-E_f；Machine Device people's avoidance sensor is capable of the range information of robot measurement preceding object thing in real time；By robot photoelectric dial sensor It is capable of the speed of service of robot measurement, the time run in conjunction with robot can obtain the distance information of robot operation.

With reference to Fig. 2, with the original position of robot and target location line as x-axis and by its D decile, every of robot Path all can use X=(x¹,x²,...,x^d,...,x^D) represent, wherein x^dRepresent the y-coordinate value of d-th Along ent, D value is bigger, Robot path planning is finer.It can be seen that the dashed path of in figure is substantially better than other two paths, robot path is advised The purpose drawn is exactly under meeting some requirements, and finds an optimum path X^*, thus by a robot path planning Problem is converted into the function optimization problem of a D dimension.

Robot path planning is the scale reducing planning space using a kind of definitiveness Sort of Method of State Space.Will Robot path planning's problem reduction becomes a 2D path planning problem, and that is, a D ties up function optimization problem, then basis Robot path planning's target, sets up its mathematical model as follows：

$F=\min {\∫}_0^L\[\mathrm{\α}B+(1-\mathrm{\α})E\]dl$

Wherein F represents generalized cost function, and L represents path, and B represents the obstruction generation to robot for the Environment Obstacles thing Valency, E represents that robot runs the energy consuming, and α ∈ [0,1] represents that robot security runs the balance system and consumed energy between Number, if the safety that robot runs is critically important, α selects larger value；If robot reaches the rapidity of target very Important, then α selects less value.Robot path planning's purpose is exactly on the premise of minimizing generalized cost function herein, Calculate the path of an optimum (or near-optimization) for robot.

The ENERGY E that robot runs consumption is relevant with the length in its path, obstruction cost B to robot for the Environment Obstacles thing It is calculated as follows,

$B=\underset{k=1}{\overset{N_B}{\Σ}}\frac{1}{d_k}$

Wherein N_BRepresent the number of barrier, with reference to Fig. 3, d_kRepresent two nodes midpoint away from k-th barrier away from From.

Charge System searching algorithm (Charged System Search, CCS) is a kind of to the coulomb in physicss Law and the new Optimizing Search technology of Newton's laws of motion simulation, it passes through the electric field force of each charged particles in colony Interact produce swarm intelligence instruct Optimizing Search.With reference to Fig. 4, each electric charge producing electric field about and can act on In other electric charges, in figure electric charge q₄Receive self charge q₁,q₂,q₃,q₅And q₆Active force final along F with joint efforts₄Direction fortune Dynamic.Each electric charge moves under other charge forces in search space, and its location updating formula and speed more new formula are such as Under：

$\begin{array}{c}{X}_{j,new}=0.5{\mathrm{rand}}_{j1}\·(1+iter/{\mathrm{iter}}_{\max })\underset{i,i\≠j}{\Σ}\left(\frac{q_i}{a^3}{r}_{ij}{i}_1+\frac{q_i}{{r_{ij}}^2}{i}_2\right)p_{ij}(X_{i,old}-X_{j,old})\\ +0.5{\mathrm{rand}}_{j2}\·(1-iter/{\mathrm{iter}}_{\max })V_{j,old}+X_{j,old}\end{array}$

V_j,new=X_j,new-X_j,old

Wherein X_j,newRepresent the new position of j-th electric charge, X_j,oldRepresent the old position of j-th electric charge, X_i,oldRepresent i-th The old position of individual electric charge, V_j,newRepresent the new speed of j-th electric charge, V_j,oldRepresent the old speed of j-th electric charge, q_iRepresent i-th The quantity of electric charge of individual electric charge, r_ijRepresent the i-th electric charge and j-th electric charge Euclidean distance in space, rand_j1And rand_j2It is uniform It is distributed in the random number of (0,1), iter represents the current iterationses of algorithm, iter_maxRepresent the maximum iteration time of algorithm, a Represent the radius that electric charge is considered as a powered spheroid, i₁And i₂It is defined as follows：

$\left\{\begin{array}{c}{i}_1=1,i_2=0\&DoubleLeftRightArrow;r_{ij}<a\\ i_1=0,i_2=1\&DoubleLeftRightArrow;r_{ij}\&GreaterEqual;a\end{array}\right.$

p_ijRepresent whether j-th electric charge is subject to the captivation of i-th electric charge, be defined as follows：

The current fitness of wherein fit (i) i-th electric charge of expression, the current fitness of fit (j) j-th electric charge of expression, Fitbest represents the best fitness of all electric charges at present, and rand is generally evenly distributed in the random number of (0,1).

Below by an instantiation, to the robot global path based on Charge System search proposed by the invention Planing method further describes.Experimental situation is 2.70Ghz, 2G internal memory, MATLAB R2012b version.

With reference to Fig. 5, a kind of robot global path planning method based on Charge System search, it implements

Step is as follows：Step 1：The foundation of robot path planning's mathematical model:

The foundation of robot environment's mathematical model：

Using a kind of definitiveness Sort of Method of State Space, reduce the scale of planning space, robot path planning is asked Topic simplification becomes a 2D path planning problem, and that is, a D ties up function optimization problem.

$F=min{\&Integral;}_0^L\&lsqb;\mathrm{\&alpha;}B+(1-\mathrm{\&alpha;})E\&rsqb;dl---\left(1\right)$

Wherein F represents generalized cost function, and L represents path, and B represents the obstruction generation to robot for the Environment Obstacles thing Valency, E represents that robot runs the energy consuming, and α ∈ [0,1] represents that robot security runs the balance system and consumed energy between Number, if the safety that robot runs is critically important, α selects larger value；If robot reaches the rapidity of target very Important, then α selects less value.

The foundation of robot path planning's optimality criterion mathematical model：

Execute safe performance indexes and the energy expenditure performance indications of task according to robot, to robot environment's barrier Obstruction cost founding mathematical models to robot.

$B=\underset{k=1}{\overset{N_B}{\&Sigma;}}\frac{1}{d_k}---\left(2\right)$

Wherein N_BRepresent the number of barrier, with reference to Fig. 3, d_kRepresent two nodes midpoint away from k-th barrier away from From.

Step 2：Initialization robot need to carry out the ambient parameter of path planning and the correlation of Charge System searching algorithm Parameter.

Parameter is set to：Electric charge population scale is N=20, and optimization dimension is D=30, and algorithm maximum iteration time is iter_max=200, robot security runs balance factor alpha=0.5 and consumed energy between, barrier number N in environment_B =5, the charge radius a=1 in location updating equation, starting point coordinate [0,0] and terminal point coordinate [60,110] that robot runs.

Step 3：Random initializtion N paths and the initial position X of each electric charge_j, for j=1,2 ..., N with just Beginning speedFor j=1,2 ..., N, rectangular coordinate system is set up for x-axis with robot original position and target location, will X-axis D decile.

Step 4：The robot path planning's mathematical model set up according to robot environment's information and in step 1, meter Calculate the obstruction cost to robot for the environment barrier in each paths, draw the fitness value fit of each electric charge, fitness is Value fitbest well, fitness bad value fitworst.

Step 5：Update the quantity of electric charge q of each electric charge_j, attraction mark p between two electric charges_ijAnd between two electric charges Euclidean distance r_ij, its more new formula is distinguished as follows：

$q_j=\frac{fit\left(j\right)-fitworst}{fitbest-fitworst},j=1,2,...,N---\left(3\right)$

$r_{ij}=\frac{\left|\right|X_i-X_j\left|\right|}{\left|\right|(X_i+X_j)/2-X_{best}\left|\right|+\mathrm{\&epsiv;}}---\left(5\right)$

Wherein X_iAnd X_jIt is i-th electric charge and j-th electric charge respectively in the position in space, X_bestIt is all electric charges at present Desired positions, i.e. the position of the corresponding electric charge of fitbest, ε is the normal number of a very little, to prevent divisor from being zero.

Step 6：Update position and the speed of each electric charge according to equation below：

$\begin{array}{c}{X}_{j,new}=0.5{\mathrm{rand}}_{j1}\&CenterDot;(1+iter/{\mathrm{iter}}_{\max })\underset{i,i\&NotEqual;j}{\&Sigma;}\left(\frac{q_i}{a^3}{r}_{ij}{i}_1+\frac{q_i}{{r_{ij}}^2}{i}_2\right)p_{ij}(X_{i,old}-X_{j,old})\\ +0.5{\mathrm{rand}}_{j2}\&CenterDot;(1-iter/{\mathrm{iter}}_{\max })V_{j,old}+X_{j,old}\end{array}---\left(6\right)$

V_j,new=X_j,new-X_j,old(7)

Then the fitness fit assessing each electric charge is recalculated according to the mathematical model of robot path planning, find out Fitness best values fitbest, bad value fitworst and fitbest corresponding Charge sites X_best,new.Robot works as Front optimal path is expressed as

Wherein X_{best,previous}Represent the optimal location of an iteration before all electric charges.

Step 7：If iterationses are more than maximum iteration time iter_max, then exit circulation, export optimal path path, no Then return to step 5 enters following iteration.

Fig. 6 is results of experimental operation, and the inventive method goes out a feasible, effective path for robot planning, becomes Circumvent the barrier in environment work(and reach impact point.

Claims (4)

1. a kind of robot global path planning method based on Charge System search, comprises the following steps:

Step 1：Set up robot path planning's mathematical model；

Step 2：Initialization robot need to carry out the ambient parameter of path planning and the relevant parameter of Charge System searching algorithm；

Step 3：The initial position of random initializtion N paths and each electric charge and speed, with robot original position and mesh Mark is set to x-axis and sets up rectangular coordinate system, by x-axis D decile,

The position defining j-th electric charge isWhereinRepresent j-th Position in d dimension for the electric charge, the position X of each electric charge_jJust represent the operating path of a robot, finally in all electric charges Optimal location be robot optimal path；

Step 4：The robot path planning's mathematical model set up according to robot environment's information and in step 1, calculates each The fitness value fit of individual electric charge, fitness best values fitbest, fitness bad value fitworst；

Step 5：According to the fitness value fit of each electric charge of acquisition, fitness best values fitbest, fitness in step 4 Bad value fitworst, updates the quantity of electric charge q of each electric charge_j, attraction mark p between two electric charges_ijAnd between two electric charges Euclidean distance r_ij；

Step 6：Quantity of electric charge q according to each electric charge obtaining in step 5_j, attraction mark p between two electric charges_ijAnd two Euclidean distance r between electric charge_ij, update position and the speed of each electric charge；Then according to the robot path set up in step 1 Mathematics for programming model recalculates the fitness of each electric charge, finds out the position X of the best electric charge of current fitness_best,new, that is, The optimal path path of robot；

Step 7：If iterationses are more than maximum iteration time iter_max, then exit circulation, export optimal path path, otherwise return Return step 5 and enter following iteration；

It is characterized in that：In step 1, by equation below, set up robot path planning's mathematical model,

$F=\min {\&Integral;}_0^L\&lsqb;\mathrm{\&alpha;}B+(1-\mathrm{\&alpha;})E\&rsqb;dl$

In formula, F represents generalized cost function, and L represents path, and B represents the obstruction cost to robot for the Environment Obstacles thing, E Represent that robot runs the energy consuming, α ∈ [0,1] represents that robot security runs the balance coefficient and consumed energy between；

Environment Obstacles thing is obtained by equation below to obstruction cost B of robot,

$B=\underset{k=1}{\overset{N_B}{\mathrm{\&Sigma;}}}\frac{1}{d_k}$

In formula, N_BRepresent the number of barrier, d_kThe distance away from k-th barrier for the midpoint of two nodes of expression.

2. the robot global path planning method based on Charge System search according to claim 1 it is characterised in that： In step 2, initialized parameter includes：Electric charge population scale N, optimizes dimension D, algorithm maximum iteration time iter_max, machine Balance factor alpha between people's safe operation and consumed energy, barrier number N in environment_B, in Charge sites renewal equation Charge radius a, starting point coordinate and terminal point coordinate that robot runs.

3. the robot global path planning method based on Charge System search according to claim 2 it is characterised in that：

In step 5, more new formula is as follows,

$q_j=\frac{fit\left(j\right)-fitworst}{fitbest-fitworst},j=1,2,...,N$

$r_{ij}=\frac{\left|\right|X_i-X_j\left|\right|}{\left|\right|(X_i+X_j)/2-X_{best}\left|\right|+\mathrm{\&epsiv;}}$

In formula, X_iAnd X_jIt is i-th electric charge and j-th electric charge respectively in the position in space, X_bestIt is the best of all electric charges at present Position, i.e. the position of the corresponding electric charge of fitbest, ε is the normal number of a very little, to prevent divisor from being zero.

4. the robot global path planning method based on Charge System search according to claim 3 it is characterised in that：

In step 6, the position of each electric charge and speed more new formula are as follows,

$\begin{array}{c}{X}_{j,new}=0.5{\mathrm{rand}}_{j1}\&CenterDot;\left(1+iter/{\mathrm{iter}}_{\max }\right)\underset{i,i\&NotEqual;j}{\mathrm{\&Sigma;}}\left(\frac{q_i}{a^3}{r}_{ij}{i}_1+\frac{q_i}{{r_{ij}}^2}{i}_2\right)p_{ij}\left(X_{i,old}-X_{j,old}\right)\\ +0.5{\mathrm{rand}}_{j2}\&CenterDot;\left(1-iter/{\mathrm{iter}}_{\max }\right)V_{j,old}+X_{j,old}\end{array}$

V_j,new=X_j,new-X_j,old.