CN104020769A - 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 in its work space, provides a safety, efficient motion path for robot completes a certain Given task.Generally speaking, robot completes the selectable path of Given task many, in practical application, often will select a path for optimum (or near-optimization) under certain criterion, conventional criterion has: shortest path, consumed energy minimum or service time is the shortest etc.Therefore, robot path planning is in fact a constrained optimization problem.

Path planning problem is an important problem of mobile robot's research field, and according to the degree of prior grasp environmental information, path planning problem can be divided into global path planning and the sensor-based local paths planning based on priori environmental information.Traditional global path planning method mainly contains Grid Method, Visual Graph method, topological approach and free-space Method etc., and these traditional methods all exist counting yield low, are not suitable for the shortcoming of high-dimensional optimization.Along with proposition and the fast development of many intelligent bionic optimized algorithms, many scholars in robot path planning field, have obtained certain effect by these algorithm application.

Charge System searching algorithm (Charged System Search, CCS) be a kind of new Optimizing Search technology that the Coulomb's law in physics and Newton's laws of motion are simulated that is derived from, be a kind of meta-heuristic algorithm, the swarm intelligence that it produces by the interaction of the electric field force of each charged particles in colony instructs Optimizing Search.

Summary of the invention

The object of the invention is to provide a kind of robot global path planning method based on Charge System search, and counting yield is high, can effectively solve robot path planning's problem under complex dynamic environment.

Realize the object of the invention technical scheme:

Based on a robot global path planning method for Charge System search, it is characterized in that:

Step 1: set up robot path planning's mathematical model;

Step 2: initialization robot need carry out the environmental parameter of path planning and the correlation parameter of Charge System searching algorithm;

Step 3: the initial position of random initializtion N paths and each electric charge and speed, taking robot reference position and target location as x axle is set up rectangular coordinate system, by x axle D decile,

The position that defines j electric charge is $X_j=(x_j^1,x_j^2,...,x_j^d,...,x_j^D),\mathrm{for\; j}=\mathrm{1,2},...,N,$ Wherein represent j the position of electric charge in d dimension, the position X of each electric charge _jjust represent the operating path of a robot, the optimal location in final all electric charges is the optimal path of robot;

Step 4: according to robot path planning's mathematical model of robot environment's information and foundation in step 1, calculate the fitness value fit of each electric charge, fitness best values fitbest, the worst value of fitness fitworst;

Step 5: according to fitness value fit, fitness best values fitbest, the worst value of the fitness fitworst of each electric charge obtaining in step 4, upgrade the quantity of electric charge q of each electric charge _j, the attraction mark p between two electric charges _ijand Euclidean distance r between two electric charges _ij;

Step 6: according to the quantity of electric charge q of the each electric charge obtaining in step 5 _j, the attraction mark p between two electric charges _ijand Euclidean distance r between two electric charges _ij, upgrade position and the speed of each electric charge; Then recalculate the fitness of each electric charge according to robot path planning's mathematical model of setting up in step 1, find out the position X of the best electric charge of current fitness _{best, new}, i.e. the optimal path path of robot;

Step 7: if iterations is greater than maximum iteration time iter _max, exiting circulation, output optimal path path, enters next iteration otherwise return to step 5.

In step 1, by following formula, set up robot path planning's mathematical model,

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

In formula, F represents generalized cost function, and L represents path, and B represents the obstruction cost of environment barrier to robot, and E represents the energy that robot operation consumes, and α ∈ [0,1] represents the balance coefficient between robot security's operation and consumed energy;

Environment Obstacles thing obtains by following formula the obstruction cost B of robot,

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

In formula, N _brepresent the number of barrier, d _krepresent that the mid point of two nodes is apart from the distance of k barrier.

In step 2, initialized parameter comprises: electric charge population scale N, optimizes dimension D, algorithm maximum iteration time iter _max, the balance factor alpha between robot security's operation and consumed energy, the barrier number N in environment _b, the charge radius a in the renewal equation of electric charge position, starting point coordinate and the terminal point coordinate of robot operation.

In step 5, more new formula is as follows,

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

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

In formula, X _iand X _jbe respectively i electric charge and j electric charge in the position in space, X _bestthe desired positions of current all electric charges, the position of the electric charge that fitbest is corresponding, ε is a very little normal number, to prevent that divisor is as zero.

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

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

V _j,new＝X _j,new-X _j,old。

The beneficial effect that the present invention has:

The present invention proposes a kind of Charge System searching algorithm model, and be successfully applied to the robot path planning's problem solving under complex environment.Be different from other bionic intelligence algorithm, the features such as the concurrency that embodies in Charge System search procedure, concertedness, self-organization, dynamic, strong robustness extremely conform to complicated robot running environment, and therefore Charge System searching algorithm can effectively solve the path planning problem under robot complex dynamic environment.

Counting yield of the present invention is high, there is good real-time and rapidity, actual robot optimal path is more approached in the path searching, making robot under the prerequisite that meets safe performance indexes and energy consumption indicators, arrive target location, is the effective technical way that solves robot path planning under complex dynamic environment.The present invention not only can be applied to robot path planning, also can be applicable to the technical field such as Path Planning for Unmanned Aircraft Vehicle, underwater robot path planning under complex environment.The present invention, for high-dimensional function optimization problem provides a very effective approach, can be widely used in the field that robot, Aeronautics and Astronautics, navigation, commercial production etc. relate to multidimensional function optimization problem.

Brief description of the drawings

Fig. 1 robot system theory diagram;

Fig. 2 robot path planning schematic diagram;

Fig. 3 Environment Obstacles thing hinders cost to robot and calculates schematic diagram;

Schematic diagram interacts between Fig. 4 electric charge;

Fig. 5 the present invention is based on the robot global path planning method program flow diagram of Charge System search;

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

Number in the figure and symbol description are as follows:

In Fig. 3: a k-the k barrier, a k-1-the k-1 barrier, a k+1-the k+1 barrier, a k+2-the k+2 barrier; (x _i-1, y _i-1the coordinate of i-1 node in)-robot path, (x _i, y _ithe coordinate of i node in)-robot path; d _kin-robot path, the mid point of i-1 node and i node is apart from the distance of k barrier, d _k-1in-robot path, the mid point of i-1 node and i node is apart from the distance of k-1 barrier, d _k+1in-robot path, the mid point of i-1 node and i node is apart from the distance of k+1 barrier, d _k+2in-robot path, the mid point of i-1 node and i node is apart from the distance of k+2 barrier;

In Fig. 4: q ₁the quantity of electric charge of-the 1 electric charge, q ₂the quantity of electric charge of-the 2 electric charge, q ₃the quantity of electric charge of-the 3 electric charge, q ₄the quantity of electric charge of-the 4 electric charge, q ₅the quantity of electric charge of-the 5 electric charge, q ₆the quantity of electric charge of-the 6 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 iterations that iter-algorithm is current, iter _maxthe maximum iteration time of-algorithm, the optimal path of path-robot, a j-the j electric charge.

Embodiment

As shown in Figure 1, robot system principle is that robot core control module is responsible for the motion control of robot and the sampling and processing of each sensor information; Robot servo motor driving system receives the control signal drive machines people motor movement of key control unit; Robot electrical measurement sensor can the current electric weight of robot measurement, and establishing the electric weight of robot in the time of initial position is E _o, the electric weight while arriving target location is E _f, energy E=E that robot operational process consumes _o-E _f; Robot obstacle-avoiding sensor is the range information of robot measurement front barrier in real time; Travelling speed that can robot measurement by robot photoelectric dial sensor, can obtain the distance information of robot operation in conjunction with the time of robot operation.

With reference to figure 2, taking the reference position of robot and target location line as x axle and by its D decile, every paths of robot is available X=(x all ¹, x ²..., x ^d..., x ^d) represent wherein x ^drepresent the y coordinate figure of d Along ent, D value is larger, and robot path planning is meticulousr.Can see that the dashed path in publishing picture is obviously better than other two paths, robot path planning's object is exactly under meeting some requirements, and finds an optimum path X ^*thereby, robot path planning's problem is converted into the function optimization problem that a D ties up.

Robot path planning utilizes a kind of determinacy Sort of Method of State Space, reduces the scale of planning space.Robot path planning's problem reduction is become to a 2D path planning problem, i.e. a D dimension function optimization problem, then according to robot path planning's target, set up its mathematical model as follows:

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

Wherein F represents generalized cost function, L represents path, B represents the obstruction cost of environment barrier to robot, E represents the energy that robot operation consumes, α ∈ [0,1] represent the balance coefficient between robot security's operation and consumed energy, if the security of robot operation is very important, α selects larger value; If it is very important that robot arrives the rapidity of target, α selects less value.Robot path planning's object is exactly under the prerequisite that minimizes generalized cost function herein, is the path of a robot optimum of calculating (or near-optimization).

The energy E that robot operation consumes is relevant with the length in its path, and Environment Obstacles thing is calculated as follows the obstruction cost B of robot,

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

Wherein N _brepresent the number of barrier, with reference to figure 3, d _krepresent that the mid point of two nodes is apart from the distance of k barrier.

Charge System searching algorithm (Charged System Search, CCS) be a kind of new Optimizing Search technology that the Coulomb's law in physics and Newton's laws of motion are simulated that is derived from, the swarm intelligence that it produces by the interaction of the electric field force of each charged particles in colony instructs Optimizing Search.With reference to figure 4, each electric charge can produce electric field and act on other electric charges around it, electric charge q in figure ₄be subject to self charge q ₁, q ₂, q ₃, q ₅and q ₆acting force final along the F that makes a concerted effort ₄direction motion.Each electric charge moves under other charge effect power in search volume, its position more new formula and speed more new formula is as follows:

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

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

Wherein X _{j, new}represent the reposition of j electric charge, X _{j, old}represent the old position of j electric charge, X _{i, old}represent the old position of i electric charge, V _{j, new}represent the new speed of j electric charge, V _{j, old}represent the old speed of j electric charge, q _irepresent the quantity of electric charge of i electric charge, r _ijrepresent i electric charge and j electric charge Euclidean distance in space, rand _j1and rand _j2be the random number that is evenly distributed on (0,1), iter represents the iterations that algorithm is current, iter _maxthe maximum iteration time that represents algorithm, a represents electric charge to be considered as the radius of a charged spheroid, i ₁and i ₂be defined as follows:

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

P _ijrepresent that whether j electric charge is subject to the attractive force of i electric charge, is defined as follows:

Wherein fit (i) represents i the fitness that electric charge is current, fit (j) represents j the fitness that electric charge is current, fitbest represents the best fitness of current all electric charges, and rand is the random number that is evenly distributed on (0,1).

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

With reference to figure 5, a kind of robot global path planning method based on Charge System search, its specific implementation

Step is as follows: step 1: the foundation of robot path planning's mathematical model:

The foundation of robot environment's mathematical model:

Utilize a kind of determinacy Sort of Method of State Space, reduce the scale of planning space, robot path planning's problem reduction is become to a 2D path planning problem, be i.e. a D dimension function optimization problem.

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

Wherein F represents generalized cost function, L represents path, B represents the obstruction cost of environment barrier to robot, E represents the energy that robot operation consumes, α ∈ [0,1] represent the balance coefficient between robot security's operation and consumed energy, if the security of robot operation is very important, α selects larger value; If it is very important that robot arrives the rapidity of target, α selects less value.

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

The safe performance indexes of executing the task according to robot and energy consume performance index, and robot environment's barrier is set up to mathematical model to the obstruction cost of robot.

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

Wherein N _brepresent the number of barrier, with reference to figure 3, d _krepresent that the mid point of two nodes is apart from the distance of k barrier.

Step 2: initialization robot need carry out the environmental parameter of path planning and the correlation parameter of Charge System searching algorithm.

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, balance factor alpha=0.5 between robot security's operation and consumed energy, the barrier number N in environment _b=5, the charge radius a=1 in the renewal equation of position, starting point coordinate [0,0] and the terminal point coordinate [60,110] of robot operation.

Step 3: the initial position X of random initializtion N paths and each electric charge _j, forj=1,2 ..., N and initial velocity taking robot reference position and target location as x axle is set up rectangular coordinate system, by x axle D decile.

Step 4: robot path planning's mathematical model of setting up according to robot environment's information and in step 1, calculate the obstruction cost of Environment Obstacles thing to robot in each paths, draw the fitness value fit of each electric charge, fitness best values fitbest, the worst value of fitness fitworst.

Step 5: the quantity of electric charge q that upgrades each electric charge _j, the attraction mark p between two electric charges _ijand Euclidean distance r between two electric charges _ij, more new formula is as follows respectively for it:

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

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

Wherein X _iand X _jbe respectively i electric charge and j electric charge in the position in space, X _bestthe desired positions of current all electric charges, the position of the electric charge that fitbest is corresponding, ε is a very little normal number, to prevent that divisor is as zero.

Step 6: position and the speed of upgrading each electric charge according to following formula:

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

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

Then the fitness fit that recalculates the each electric charge of assessment according to robot path planning's mathematical model, finds out fitness best values fitbest, the electric charge position X that the worst value fitworst and fitbest are corresponding _{best, new}.The current optimal path of robot is expressed as

Wherein X _{best, previous}represent the optimal location of the front iteration of all electric charges.

Step 7: if iterations is greater than maximum iteration time iter _max, exiting circulation, output optimal path path, enters next iteration otherwise return to step 5.

Fig. 6 is results of experimental operation, and the inventive method is that robot planning goes out feasible, an effective path, successfully keeps away the barrier in environment and arrives impact point.

Claims (5)

1. the robot global path planning method based on Charge System search, is characterized in that:

Step 1: set up robot path planning's mathematical model;

Step 2: initialization robot need carry out the environmental parameter of path planning and the correlation parameter of Charge System searching algorithm;

Step 3: the initial position of random initializtion N paths and each electric charge and speed, taking robot reference position and target location as x axle is set up rectangular coordinate system, by x axle D decile,

The position that defines j electric charge is wherein represent j the position of electric charge in d dimension, the position X of each electric charge _jjust represent the operating path of a robot, the optimal location in final all electric charges is the optimal path of robot;

Step 4: according to robot path planning's mathematical model of robot environment's information and foundation in step 1, calculate the fitness value fit of each electric charge, fitness best values fitbest, the worst value of fitness fitworst;

Step 5: according to fitness value fit, fitness best values fitbest, the worst value of the fitness fitworst of each electric charge obtaining in step 4, upgrade the quantity of electric charge q of each electric charge _j, the attraction mark p between two electric charges _ijand Euclidean distance r between two electric charges _ij;

Step 6: according to the quantity of electric charge q of the each electric charge obtaining in step 5 _j, the attraction mark p between two electric charges _ijand Euclidean distance r between two electric charges _ij, upgrade position and the speed of each electric charge; Then recalculate the fitness of each electric charge according to robot path planning's mathematical model of setting up in step 1, find out the position X of the best electric charge of current fitness _{best, new}, i.e. the optimal path path of robot;

Step 7: if iterations is greater than maximum iteration time iter _max, exiting circulation, output optimal path path, enters next iteration otherwise return to step 5.

2. the robot global path planning method based on Charge System search according to claim 1, is characterized in that: in step 1, by following formula, set up robot path planning's mathematical model,

In formula, F represents generalized cost function, and L represents path, and B represents the obstruction cost of environment barrier to robot, and E represents the energy that robot operation consumes, and α ∈ [0,1] represents the balance coefficient between robot security's operation and consumed energy;

Environment Obstacles thing obtains by following formula the obstruction cost B of robot,

In formula, N _brepresent the number of barrier, d _krepresent that the mid point of two nodes is apart from the distance of k barrier.

3. the robot global path planning method based on Charge System search according to claim 2, is characterized in that: in step 2, initialized parameter comprises: electric charge population scale N, optimizes dimension D, algorithm maximum iteration time iter _max, the balance factor alpha between robot security's operation and consumed energy, the barrier number N in environment _b, the charge radius a in the renewal equation of electric charge position, starting point coordinate and the terminal point coordinate of robot operation.

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

In step 5, more new formula is as follows,

In formula, X _iand X _jbe respectively i electric charge and j electric charge in the position in space, X _bestthe desired positions of current all electric charges, the position of the electric charge that fitbest is corresponding, ε is a very little normal number, to prevent that divisor is as zero.

5. the robot global path planning method based on Charge System search according to claim 4, is characterized in that:

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

V _j,new＝X _j,new-X _j,old。