CN103336526A - Robot path planning method based on coevolution particle swarm rolling optimization - Google Patents

Abstract

The invention discloses a robot path planning method adopting multi-subgroup coevolution particle swarm optimization to perform real-time rolling optimization obstacle avoidance processing, which can enable a robot to realize autonomous navigation in a working condition with stationary obstacles at unknown positions. The method has the following advantages: the method can realize autonomous navigation of the robot in a working condition with stationary obstacle at unknown position; the method introduces group mass center strategy to the multi-subgroup coevolution, so as to increase the search capacity of the population; compared with a conventional path planning method which adopts PSO (Particle Swarm Optimization) and CPSO (Coevolution Particle Swarm Optimization), the method provided by the invention adopts survival of the fittest strategy, thereby being more advantageous in respect to rate and accuracy of evolution convergence; the method provided by the invention can real-timely plan an effective obstacle avoidance path in a complex obstacle environment, and has the advantages of good robustness, high solving efficiency, and the like.

Description

Robot path planning method based on coevolution population rolling optimization

Technical field

The invention belongs to the real-time planning technology of robot path field, be specifically related to a kind of robot path planning method based on coevolution population rolling optimization.

Background technology

Path planning refers to by performance requirements such as distance, consuming time or securities, the safe clear path of searching machine people between from the starting point to the impact point, and it is robot field's important topic.According to the understanding to environment knowledge, path planning can be divided into two kinds of the path planning of known environment and the path plannings under the circumstances not known, the former is also referred to as global path planning; The latter then requires robot to utilize the sensor of self to survey the interior information of limited range on every side, constantly carries out dynamic path planning, is also referred to as online path planning or local paths planning.Handle owing to need to realize the online barrier of keeping away, therefore online path planning than the global path planning complexity many.

Current, local paths planning method mainly contains: Artificial Potential Field method, rolling window law of planning and intelligent algorithm law of planning etc.Artificial Potential Field method algorithm structure is simple but exist local extremum, and robot can be absorbed in local extremum and fluctuate; Adopting in the rolling window planing method exists deadlock now to look like.In recent years, along with going deep into that bionic intelligence is studied, many intelligent optimization algorithms are used for the robot path planning gradually, as genetic algorithm, ant group algorithm and particle cluster algorithm.

Summary of the invention

In order to overcome the defective that exists in the prior art, provide a kind of robot that can make under the working environment with static-obstacle thing and the unknown of barrier position, to realize independent navigation, adopt many subgroups coevolution particle cluster algorithm to carry out real-time rolling optimization and keep away the robot path planning method that barrier is handled.The concrete technical scheme of the present invention is as follows:

Based on the robot path planning method of coevolution population rolling optimization, be used for the walking path of a robot is planned, make robot avoid some stationary obstruction in the working environment, comprise the steps:

Step S1 carries out modeling to working environment;

Step S2, the distance between the current point of detection machine people and the impact point if distance, is then thought the arrival impact point less than default distance error, is exported the terminal point collection of each time planning, the acquisition programme path; Otherwise change step S3 over to;

Step S3 obtains current optimal movement orientation angle according to the current point of robot and impact point;

Step S4 according to default robot field-of-view angle, divides the GOTO field of robot; Determine that according to current optimal movement orientation angle the motion of current robot is towards the GOTO field at place;

Step S5, if clear in the view field of current robot, be that limit is set up the local pole coordinate system with current point, the local endpoint that obtains this planning according to default radius of view and current optimal movement orientation angle, and deposit local endpoint in the terminal point collection, as the current point of planning next time, change local endpoint over to step S2; If in the view field of current robot barrier is arranged, changes step S6 over to;

Step S6 adopts the mode of interative computation to keep away barrier based on the coevolution particle cluster algorithm and handles; Step S6 further comprises:

Step S61 is divided into several segmentations with radius of view, determines several segment movement orientation angles simultaneously, and the segment movement orientation angle is less than field-of-view angle; Some particles are set, each particle is encoded to the sequence of being made up of some segment movement orientation angles, some particles are formed an initial population, and initial population is carried out initialization, obtain an initial population; If the initialization success is divided into several subgroups with initial population, change step S62 over to; If the initialization failure changes step S7 over to;

Step S62 calculates the adaptive value of each particle in each subgroup, and the particle in each subgroup is sorted by adaptive value, selects the less particle of adaptive value in each subgroup by default gene-ratio again, forms a gene pool;

Step S63 according to the adaptive value of each particle in each subgroup, obtains the optimum solution of each subgroup, and tries to achieve a population center of mass point according to optimum solution;

Step S64 upgrades particle's velocity and position in each subgroup; If upgrade successfully, then change step S65 over to, if upgrade failure, change step S7 over to;

Step S65 upgrades the adaptive value of particle in each subgroup, the individual optimum solution of particle and the globally optimal solution of subgroup;

Step S66 judges whether the evolution cycle that reaches default, does not then directly change step S67 over to if reach evolution cycle; Otherwise upgrade subgroup and gene pool, new population center of mass point more changes step S67 again over to then;

Step S67 judges whether the maximum iteration time that reaches default, if reached maximum iteration time, termination of iterations then obtains the local endpoint of this planning, and local endpoint is added terminal point concentrates, as the current point of planning next time, change local endpoint over to step S2; If do not reach maximum iteration time, then change step S64 over to and continue iteration;

Step S7 keeps away the barrier failure and handles, and changes robot over to next GOTO field, returns step S61 and keeps away barrier; If robot all keeps away the barrier failure at all GOTO fields, then can't realize keeping away barrier, failure is jumped out.

As prioritization scheme, the working environment modeling of step S1 specifically comprises the steps:

Step S11 ignores the elevation information in the working environment, sets up two-dimensional coordinate system;

Step S12, the view field of establishing robot for (r, θ), wherein r is radius of view, θ is field-of-view angle; Obtain the starting point S of robot and the coordinate of impact point G; With convex polygon barrier is described, coordinate the unknown of barrier;

Step S13 carries out expandedly to barrier according to the size of robot, and robot handled as a particle;

Step S14, it is 1 that current scrolling planning number of times t is set, and current some P of robot is set ^tBe starting point S, and with current some P ^tDeposit terminal point collection { P} in.

As prioritization scheme, step S3 is specially:

Current optimal movement orientation angle

Refer to current motion towards with the angle of two-dimensional coordinate system x axle, current motion is oriented current some P ^tRectilinear direction to impact point G; Current optimal movement orientation angle

Computing formula as the formula (1):

(1)

Wherein, t is current scrolling planning number of times, if

Then order

G _xBe the coordinate figure of described impact point G at the x axle, G _yBe the coordinate figure of described impact point G at the y axle, P _x ^tBe described current some P ^tAt the coordinate figure of x axle, P _y ^tBe described current some P ^tCoordinate figure at the y axle.

As prioritization scheme, step S4 is specially:

According to field-of-view angle θ, the GOTO field of robot is divided into K=2 π/θ, according to current optimal movement orientation angle Determine the current motion of robot towards the GOTO field k at place, current motion towards the computing formula of the GOTO field k at place as the formula (2):

(2)

Wherein, if k=0 then makes k=K, wherein ceil () is the function that rounds up, and mod () is for getting complementary function.

As prioritization scheme, step S5 is specially:

Local endpoint P ^t' computing formula as the formula (3):

(3)

Wherein, P ^t' _xBe described local endpoint P ^t' at the coordinate figure of x axle, P ^t' _yBe described local endpoint P ^t' at the coordinate figure of y axle, P _x ^tBe described current some P ^tAt the coordinate figure of x axle, P _y ^tBe described current some P ^tCoordinate figure at the y axle; With described local endpoint P ^t' deposit described terminal point collection in and { behind the P}, make t=t+1.

As prioritization scheme, step S61 further comprises the steps:

Step S611 is divided into the m section with radius of view r, obtains a segmentation radius λ, determines m segment movement orientation angle at random

Wherein, I=1,2 ... m obtains some segmented paths dist _i, by some segmented paths dist _iConnect to form the path length overall dist that robot avoiding obstacles in radius of view r scope need pass through _t

Step S612 arranges the interative computation that some particles are used for carrying out particle cluster algorithm, and particle is encoded to by some segment movement orientation angles The sequence of forming, the dimension of establishing some particles is m, according to the GOTO field k of current motion towards the place, the scope that defines some particles is [(k-1) θ, k θ], wherein, and k=1,2 ... K, K=2 π/θ;

Step S613 determines path length overall dist _t, path length overall dist _tComputing formula as the formula (4):

${\mathrm{dist}}_t=\mathrm{\λ}+\underset{i-2}{\overset{m}{\mathrm{\Σ}}}{\mathrm{dist}}_i=r/m+\underset{i=2}{\overset{m}{\mathrm{\Σ}}}\sqrt{{\left((i-1)\mathrm{\λ}\right)}^2+{\left(\mathrm{i\λ}\right)}^2+2\mathrm{i\λ}(i-1)\mathrm{\λ}\cos (x_i-x_{i-1})}$ (4)

Wherein, i=2,3 ..., m; λ=r/m, namely radius of view r makes the section length behind the branches such as m; x _iI dimension for the back particle of encoding;

Obtain local endpoint P ^t' to the air line distance of impact point G

Air line distance Computing formula as the formula (5):

${\mathrm{dist}}_{P^{t\′}G}=\sqrt{{({P^{t\′}}_x-G_x)}^2+{\left({P^{t\′}}_y{-G}_y\right)}^2}$ (5)

According to path length overall dist _tAnd air line distance

Set an adaptive value function, the adaptive value function as the formula (6):

$\mathrm{Fit}={\mathrm{\ω}}_1{\mathrm{dist}}_t+{\mathrm{\ω}}_2{\mathrm{dist}}_{P^{t\′}G}$ (6)

ω wherein ₁With ω ₂Be weight coefficient;

Step S614 forms an initial population with some particles, and initial population is carried out initialization; Initialization requires current some P from this planning ^tBeginning is with segmented paths dist _iFor there not being barrier in the view field of radius of view, otherwise the initialization failure;

Step S615 if the initialization success is divided into s subgroup with initial population, according to the GOTO field k of current motion towards the place, defines the scope [x of some particles _Min, x _Max] be [(k-1) θ, k θ]; The maximal rate that particle is set is 0.1 times of particle range.

As prioritization scheme, the initialization among the step S614 is specially:

The setting original dimensions is that m, initial scale are N, carry out initialization before, the population scale of initial population is set much larger than initial scale N; The maximum initialization times p of particle is set _MaxWith the maximum initialization times d of dimension _MaxIf certain one dimension of a certain particle is being made d in the initial population _MaxSuccess not yet then reinitializes current dimension after the inferior initialization; If this particle is through p _MaxInferior initialization is still failed, and then thinks current particle initialization failure;

Abandon the particle of initialization failure, the initialized particle of success is formed initial population; If the scale of initial population reaches initial scale N, then initialization success; Otherwise, the initialization failure.

As prioritization scheme, population center of mass point P among the step S63 _cDefinition as the formula (7):

$P_c(R+1)=\frac{1}{s}\underset{1}{\overset{s}{\mathrm{\Σ}}}{p}_{\mathrm{gd}}^i\left(R\right)i=\mathrm{1,2},...,s$ (7)

Wherein, R is evolution cycle,

It is the globally optimal solution of i subgroup.

As prioritization scheme, step S64 is specially:

Upgrade particle's velocity and position in each subgroup according to formula (8) and formula (9):

${v_i}^j(\mathrm{iter}+1)=\mathrm{\&omega;}\&times;{v_i}^j\left(\mathrm{iter}\right)+c_1{\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="HDA00003380518400041.png"\; state="\{\{state\}\}">r</figure-callout>}}_1\&times;({P_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_2{r}_2\&times;({g_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_3{r}_3({P_c}^R-{x_i}^j\left(\mathrm{iter}\right))$

(8)

$x_i^j(\mathrm{iter}+1)=x_i^j\left(\mathrm{iter}\right)+v_i^j(\mathrm{iter}+1)$ (9)

Wherein iter is the population evolutionary generation, and j is the subgroup mark, and i is particle mark in the subgroup,

Be i particle's velocity in j the subgroup; P _i ^jIt is the historical optimum solution of i particle in j the subgroup;

Optimum solution for subgroup, current particle place; c ₁And c ₂Be the study factor; c ₃For exchanging the factor; r ₁, r ₂, r ₃Be the random number between [0,1]; ω is inertia weight, and inertia weight ω adopts the linear decrease strategy, as the formula (10):

ω(iter)=ω _max-iter×(ω _max-ω _min)/iter _max (10)

Particle's velocity and location boundary processing are comprised: if the d of a certain particle i dimension speed among the j of subgroup $v_{i,j}^d>v_{\max },$ Then order $v_{i,j}^d=v_{\max };$ If $v_{i,j}^d<v_{\min },$ Then order $v_{i,j}^d=v_{\min };$ If $x_{i,j}^d>x_{\max },$ Then order $x_{i,j}^d=x_{\max };$ If $x_{i,j}^d<x_{\min },$ Then $x_{i,j}^d=x_{\min };$

Each dimension after the particle position renewal is required to comprise: from current some P ^tBeginning is with segmented paths dist _iFor there not being barrier in the view field of radius of view, if there is then current dimension renewal failure of barrier; If certain one dimension of a certain particle is made d _MaxInferior renewal is success not yet, and then the current dimension of this particle is upgraded failure, if a certain particle is made p _MaxInferior renewal is success not yet, and then this particle upgrades failure; If all subgroups all upgrade failure, termination of iterations and change step S7 over to then.

As prioritization scheme, step S66 is specially:

Judge whether the evolution cycle R that reaches default, if reach then at first according to default dead ratio, from gene pool, select particle to substitute the most bad particle in each subgroup at random; Again according to adaptive value, if the adaptive value of a certain particle is less than the adaptive value of a certain particle in the gene pool in each subgroup, then with the corresponding particle in this particle replacement gene pool in each subgroup; Final updating population barycenter P _c

As prioritization scheme, the method that among the step S7 robot is changed over to next GOTO field is: if described current optimal movement orientation angle

The local endpoint P of this Rolling Planning ^t' be positioned at current some P ^tThe y axle increase to, then each GOTO field is rotated counterclockwise test, otherwise each GOTO field is made the dextrorotation transfer to test.

Compared with prior art, the present invention has following beneficial effect:

(1) the present invention can realize the independent navigation of robot under the working environment with static-obstacle thing and the unknown of barrier position;

(2) the present invention introduces colony's barycenter strategy in the coevolution of many subgroups, has improved the search capability of population;

(3) with respect to the paths planning method of existing employing standard particle group algorithm (PSO) and coevolution particle cluster algorithm (CPSO), the present invention has adopted survival of the fittest strategy, has more superiority in evolution speed of convergence and precision;

(4) the present invention has to cook up in real time in the complex barrier substance environment and effectively keeps away the barrier path, have robustness good, find the solution the efficient advantages of higher.

Description of drawings

Fig. 1 is overview flow chart of the present invention;

Fig. 2 is the process flow diagram of step S6 of the present invention;

Fig. 3 is the synoptic diagram of working environment modeling among the step S1 of the present invention;

Fig. 4 keeps away the synoptic diagram of barrier for segmentation among the step S6 of the present invention;

Fig. 5 is the fall into a trap synoptic diagram of point counting section path length overall of step S63 of the present invention;

Fig. 6 is for turning to the synoptic diagram of domain test among the step S7 of the present invention;

Fig. 7 is the path planning result in the embodiment of the invention 1 typical obstacle environment;

Fig. 8 carries out the comparison diagram of path planning for adopting three kinds of algorithms of different;

Fig. 9 is the path planning result in the embodiment of the invention 1 slit environment;

Figure 10 is the path planning result in the embodiment of the invention 1 strip obstacle environment;

Figure 11 is the path planning result in the embodiment of the invention 1 spill obstacle environment.

Embodiment

Describe the present invention below in conjunction with accompanying drawing in detail in the mode of embodiment.

Embodiment 1:

As shown in Figure 1, based on the robot path planning method of coevolution population rolling optimization, be used for the walking path of a robot is planned, make robot avoid some stationary obstruction in the working environment, comprise the steps:

Step S1 carries out modeling to working environment, as shown in Figure 3, specifically comprises the steps:

Step S11 ignores the elevation information in the working environment, sets up two-dimensional coordinate system.This step has been ignored the elevation information of robot and barrier, the working environment of robot is described as the environment of a two-dimentional bounded.

Step S12, the view field of establishing robot for (r, θ), wherein r is radius of view, θ is field-of-view angle; Obtain the starting point S of robot and the coordinate of impact point G; In two-dimensional coordinate system, with convex polygon barrier is described, coordinate the unknown of barrier.

Step S13 carries out expandedly to barrier according to the size of robot, and robot handled as a particle.Shape and the structure of ignoring robot itself in path planning of the present invention are regarded it as particle and are handled.

Step S14, it is 1 that current scrolling planning number of times t is set, and current some P of robot is set ^tBe starting point S, and with current some P ^tDeposit terminal point collection { P} in.

Step S2, current some P of detection machine people ^tAnd the distance between the impact point G, if distance, is then thought the arrival impact point less than default distance error ξ, export terminal point collection { P}, the acquisition programme path of each time planning; Otherwise change step S3 over to;

Step S3 is according to current some P of robot ^tObtain current optimal movement orientation angle with impact point G Current optimal movement orientation angle

Refer to current motion towards with the angle of two-dimensional coordinate system x axle, current motion is oriented current some P ^tRectilinear direction to impact point G; Current optimal movement orientation angle

Computing formula as the formula (1):

(1)

Wherein, t is current scrolling planning number of times, if

Then order

G _xBe the coordinate figure of described impact point G at the x axle, G _yBe the coordinate figure of described impact point G at the y axle, P _x ^tBe described current some P ^tAt the coordinate figure of x axle, P _y ^tBe described current some P ^tCoordinate figure at the y axle.

Step S4 according to field-of-view angle θ, is divided into K=2 π/θ with the GOTO field of robot, according to current optimal movement orientation angle

Determine the current motion of robot towards the GOTO field k at place, current motion towards the computing formula of the GOTO field k at place as the formula (2):

(2)

Wherein, if k=0 then makes k=K, wherein ceil () is the function that rounds up, and mod () is for getting complementary function.

Step S5 is if clear in the view field of current robot is that limit is set up the local pole coordinate system with current point, according to default radius of view r and current optimal movement orientation angle

Obtain the local endpoint P of this planning ^t', local endpoint P ^t' computing formula as the formula (3):

(3)

Wherein, P ^t' _xBe described local endpoint P ^t' at the coordinate figure of x axle, P ^t' _yBe described local endpoint P ^t' at the coordinate figure of y axle, P _x ^tBe described current some P ^tAt the coordinate figure of x axle, P _y ^tBe described current some P ^tCoordinate figure at the y axle;

With local endpoint P ^t' { P} makes t=t+1, and with local endpoint P to deposit the terminal point collection in ^t' as the current some P that plans next time ^t, change step S2 over to; If in the view field of current robot barrier is arranged, changes step S6 over to.

As shown in Figure 2, step S6 adopts the mode of interative computation to keep away barrier based on the coevolution particle cluster algorithm and handles; Specifically comprise the steps:

Step S61, the initialization of population step specifically comprises the steps:

Step S611 is divided into the m section with radius of view r, obtains a segmentation radius λ, determines m segment movement orientation angle simultaneously at random

, wherein, I=1,2 ... m, segment movement orientation angle

Field-of-view angle θ less than robot; This step be attempt with less segmentation and less angle attempt " around " the mistake barrier, as shown in Figure 4, according to segmentation radius λ and segment movement orientation angle

Can in the view field of robot, obtain m waypoint; Distance between adjacent two waypoints is segmented paths dist _iThereby, obtain m segmented paths dist _i, by m segmented paths dist _iConnect to form the path length overall dist that robot avoiding obstacles in radius of view r scope need pass through _t

Step S612 arranges the interative computation that some particles are used for carrying out particle cluster algorithm, and particle is encoded to by some segment movement orientation angles The sequence of forming, the dimension of establishing some particles is m, according to the GOTO field k of current motion towards the place, the scope that defines some particles is [(k-1) θ, k θ], (k=1,2 ... K, K=2 π/θ).

Step S613 as shown in Figure 5, can determine path length overall dist according to the cosine law _t, path length overall dist _tComputing formula as the formula (4):

${\mathrm{dist}}_t=\mathrm{\&lambda;}+\underset{i-2}{\overset{m}{\mathrm{\&Sigma;}}}{\mathrm{dist}}_i=r/m+\underset{i=2}{\overset{m}{\mathrm{\&Sigma;}}}\sqrt{{\left((i-1)\mathrm{\&lambda;}\right)}^2+{\left(\mathrm{i\&lambda;}\right)}^2+2\mathrm{i\&lambda;}(i-1)\mathrm{\&lambda;}\cos (x_i-x_{i-1})}$ (4)

Wherein, i=2,3 ..., m; λ=r/m, namely radius of view r makes the segmentation radius behind the branches such as m; First segmented paths dist ₁=λ, x _iI dimension for the back particle of encoding;

Obtain local endpoint P ^t' to the air line distance of impact point G Air line distance

Computing formula as the formula (5):

${\mathrm{dist}}_{P^{t\&prime;}G}=\sqrt{{({P^{t\&prime;}}_x-G_x)}^2+{\left({P^{t\&prime;}}_y{-G}_y\right)}^2}$ (5)

According to path length overall dist _tAnd air line distance Set an adaptive value function, the adaptive value function as the formula (6):

$\mathrm{Fit}={\mathrm{\&omega;}}_1{\mathrm{dist}}_t+{\mathrm{\&omega;}}_2{\mathrm{dist}}_{P^{t\&prime;}G}$ (6)

ω wherein ₁With ω ₂Be weight coefficient.

Step S614 forms an initial population with some particles, and initial population is carried out initialization; Initialization requires current some P from this planning ^tBeginning is with segmented paths dist _iFor there not being barrier in the view field of radius of view, otherwise the initialization failure; Step S614 further comprises:

The setting original dimensions is that m, initial scale are N, carry out initialization before, the population scale of initial population is set much larger than initial scale N; The maximum initialization times p of particle is set _MaxWith the maximum initialization times d of dimension _MaxIf certain one dimension of a certain particle is being made d in the initial population _MaxSuccess not yet then reinitializes current dimension after the inferior initialization; If this particle is through p _MaxInferior initialization is still failed, and then thinks current particle initialization failure;

Abandon the particle of initialization failure, the initialized particle of success is formed initial population; If the scale of initial population reaches initial scale N, then initialization success changes next step over to; Otherwise the initialization failure changes step S7 over to.

Step S615 if the initialization success is divided into s subgroup with initial population, according to the GOTO field k of current motion towards the place, defines the scope [x of some particles _Min, x _Max] be [(k-1) θ, k θ]; The maximal rate that particle is set is 0.1 times of particle range.

Step S62 calculates the adaptive value of each particle in each subgroup, and the particle in each subgroup is sorted by adaptive value, selects the higher particle of adaptive value in each subgroup by default gene-ratio again, forms a gene pool.This step has adopted " survival of the fittest " strategy, selects " elite " particle optimum in each subgroup to create gene pool by certain gene-ratio, and the particle in this gene pool is less " elite " particle of adaptive value.

Step S63 according to the adaptive value of each particle in each subgroup, obtains the optimum solution of each subgroup And try to achieve a population center of mass point P according to optimum solution _cPopulation center of mass point P _cDefinition as the formula (7):

$P_c(R+1)=\frac{1}{s}\underset{1}{\overset{s}{\mathrm{\&Sigma;}}}{p}_{\mathrm{gd}}^i\left(R\right)i=\mathrm{1,2},...,s$ (7)

Wherein, R is evolution cycle,

It is the globally optimal solution of i subgroup.Population center of mass point P _cBe the mean value of each subgroup optimum solution, represented the desired center of whole population, it upgrades with evolution cycle, no speed component.The population center of mass point is the average of the globally optimal solution of each subgroup, it is a kind of desirable barycenter, it is not the particle of a reality, the reason of introducing barycenter is, it is desirable optimum to follow individual historical optimum, subgroup global optimum and population when particle circles in the air in solution space, because a population is desirable optimum than having Duoed in the standard particle group evolutionary equation, it is when so particle flies in theory that directivity is clearer and more definite, can reduce the randomness of particle flight, certain " interchange " also arranged between each subgroup, therefore can significantly improve the search capability of population.

Step S64, from then on step begins the interative computation that rolls, and upgrades particle's velocity and position in each subgroup according to formula (8) and formula (9):

${v_i}^j(\mathrm{iter}+1)=\mathrm{\&omega;}\&times;{v_i}^j\left(\mathrm{iter}\right)+c_1{\mathrm{<figure-callout\; id="1"\; label="r"\; filenames="HDA00003380518400041.png"\; state="\{\{state\}\}">r</figure-callout>}}_1\&times;({P_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_2{r}_2\&times;({g_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_3{r}_3({P_c}^R-{x_i}^j\left(\mathrm{iter}\right))$

(8)

$x_i^j(\mathrm{iter}+1)=x_i^j\left(\mathrm{iter}\right)+v_i^j(\mathrm{iter}+1)$ (9)

Wherein iter is the population evolutionary generation, and j is the subgroup mark, and i is particle mark in the subgroup,

Be i particle's velocity in j the subgroup; P _i ^jIt is the historical optimum solution of i particle in j the subgroup;

Optimum solution for subgroup, current particle place; c ₁And c ₂Be the study factor; c ₃For exchanging the factor; r ₁, r ₂, r ₃Be the random number between [0,1]; ω is inertia weight, and inertia weight ω adopts the linear decrease strategy, as the formula (10):

ω(iter)=ω _max-iter×(ω _max-ω _min)/iter _max (10)

Particle's velocity and location boundary processing are comprised: if the d of a certain particle i dimension speed among the j of subgroup $v_{i,j}^d>v_{\max },$ Then order $v_{i,j}^d=v_{\max };$ If $v_{i,j}^d<v_{\min },$ Then order $v_{i,j}^d=v_{\min };$ If $x_{i,j}^d>x_{\max },$ Then order $x_{i,j}^d=x_{\max };$ If $x_{i,j}^d<x_{\min },$ Then $x_{i,j}^d=x_{\min };$

Each dimension after the particle position renewal is required to comprise: from current some P ^tBeginning is with segmented paths dist _iFor there not being barrier in the view field of radius of view, if there is then current dimension renewal failure of barrier; If certain one dimension of a certain particle is made d _MaxInferior renewal is success not yet, and then the current dimension of this particle is upgraded failure, if a certain particle is made p _MaxInferior renewal is success not yet, and then this particle upgrades failure; If all subgroups all upgrade failure, termination of iterations and change step S7 over to then; Successfully then change step S65 over to if upgrade.

Step S65 upgrades the adaptive value of particle in each subgroup, the individual optimum solution of particle and the globally optimal solution of subgroup.

Step S66 judges whether the evolution cycle R that reaches default, does not then directly change step S67 over to if reach evolution cycle R; If reach evolution cycle R then at first according to default dead ratio, from gene pool, select particle to substitute the most bad particle in each subgroup at random; If the adaptive value of a certain particle is less than the adaptive value of a certain particle in the gene pool in each subgroup, then with the corresponding particle in this particle replacement gene pool in each subgroup; New population barycenter P more then _cChange step S67 at last over to.This step has adopted " survival of the fittest " strategy, and the adaptive value of particle is more little, and then this particle is more outstanding.Eliminate the higher particle of a part of adaptive value in each subgroup according to dead ratio earlier, from gene pool, select particle to replace; Eliminate the higher particle of adaptive value in the gene pool again, from each subgroup, choose outstanding particle and replace; Thereby make the particle in gene pool and each subgroup all keep less adaptive value, make the speed of convergence of evolution faster, precision is higher.

Step S67 judges whether the maximum iteration time iter that reaches default _Max, if reached maximum iteration time iter _Max, termination of iterations then, the local endpoint P that obtains this planning according to globally optimal solution and the radius of view r of all subgroups ^t', and with local endpoint P ^t' add the terminal point collection and { among the P}, make t=t+1, with local endpoint P ^t' as the current some P that plans next time ^t, change step S2 over to; If do not reach maximum iteration time iter _Max, then change step S64 over to and continue iteration;

Step S7 keeps away the barrier failure and handles, and changes robot over to next GOTO field, and the method that changes next GOTO field over to is: if current optimal movement orientation angle

The local endpoint P of this Rolling Planning ^t' be positioned at current some P ^tThe y axle increase to, then each GOTO field is rotated counterclockwise test, namely k=mod (k+i, K), i=1,2 ... K-1; Otherwise each GOTO field is made the dextrorotation transfer to test.When turning clockwise, to k～1 GOTO field press k=mod (k-i, K) conversion GOTO field, i=0 wherein, 1,2 ... k-1; To K～k GOTO field press k=mod (K-i, K) conversion GOTO field, i=0 wherein, 1,2 ... K-k-1, as the formula (11):

After changing next GOTO field over to, return step S61 and keep away barrier, as shown in Figure 6; If robot all keeps away the barrier failure at all GOTO fields, then can't realize keeping away barrier, failure is jumped out.

In the present embodiment, select following parameter to further specify.

(1) population parameter: get initial scale N=20, subcluster number s=4, subgroup scale are 5, particle dimension m=10, learning and exchange factor c ₁=c ₂=c ₃=2, inertia weight ω drops to 0.4 from 0.9 linearity, weight coefficient ω in the adaptive value function ₁=ω ₂=1, maximum iteration time iter _Max=100; Population evolution cycle R=4, the maximum initialization times p of particle _MaxWith the maximum initialization times d of dimension _MaxBe p _Max=d _Max=20, gene-ratio is 0.2, and dead ratio is 0.2.

(2) robot running parameter: radius of view r=5, field-of-view angle is θ=π/3, GOTO field is K=2 π/θ=6, distance error ξ=2.

The establishment size is 100 * 100 typical barrier working environment, wherein exists several static-obstacle things, and it is S (10,10) that starting point is set, and impact point is G (90,90), and the path of cooking up as shown in Figure 7.

Systematic parameter is identical, selection closes on the point (45 of barrier, 29), use standard particle group's algorithm (PSO), coevolution particle cluster algorithm (CPSO) to adopt many subgroups coevolution particle cluster algorithm (MCPSO) of " survival of the fittest " strategy to keep away the barrier test respectively with the present invention, the adaptive value change curve of three kinds of algorithms as shown in Figure 8, the MCPSO algorithm has more superiority in evolution speed of convergence and precision as seen from the figure.

Under special obstacle environment, as the slit environment among Fig. 9, huge long strip type obstacle environment among Figure 10 and the matrix obstacle environment among Figure 11 adopt this programme also can be implemented in thread path planning effectively, and Fig. 7, Fig. 8 and Fig. 9 are seen in the path of cooking up.

More than disclosed only be several specific embodiments of the application, but the not limited thereto any those skilled in the art of the application can think variation, all should drop in the application's the protection domain.

Claims (11)

1. based on the robot path planning method of coevolution population rolling optimization, be used for the walking path of a robot is planned, make robot avoid some stationary obstruction in the working environment, it is characterized in that, comprise the steps:

Step S1 carries out modeling to described working environment;

Step S2 detects the distance between the current point of described robot and the impact point, if described distance, is then thought the arrival impact point less than default distance error, exports the terminal point collection of each time planning, the acquisition programme path; Otherwise change step S3 over to;

Step S3 obtains current optimal movement orientation angle according to the current point of described robot and impact point;

Step S4 according to default robot field-of-view angle, divides the GOTO field of robot; Determine that according to described current optimal movement orientation angle the motion of current described robot is towards the GOTO field at place;

Step S5, if clear in the view field of current described robot, be that limit is set up the local pole coordinate system with described current point, the local endpoint that obtains this planning according to default radius of view and described current optimal movement orientation angle, and deposit described local endpoint in described terminal point collection, as the current point of planning next time, change described local endpoint over to step S2; If in the view field of current described robot barrier is arranged, changes step S6 over to;

Step S6 adopts the mode of interative computation to keep away barrier based on the coevolution particle cluster algorithm and handles; Described step S6 further comprises:

Step S61 is divided into several segmentations with described radius of view, determines several segment movement orientation angles simultaneously, and described segment movement orientation angle is less than described field-of-view angle; Some particles are set, each described particle is encoded to the sequence of being made up of some described segment movement orientation angles, some described particles are formed an initial population, and described initial population is carried out initialization, obtain an initial population; If the initialization success is divided into several subgroups with described initial population, change step S62 over to; If the initialization failure changes step S7 over to;

Step S62 calculates the adaptive value of each particle in each described subgroup, and the particle in each described subgroup is sorted by adaptive value, selects the less particle of adaptive value in each described subgroup by default gene-ratio again, forms a gene pool;

Step S63 according to the adaptive value of each particle in each described subgroup, obtains the optimum solution of each described subgroup, and tries to achieve a population center of mass point according to described optimum solution;

Step S64 upgrades particle's velocity and position in each described subgroup; If upgrade successfully, then change step S65 over to, if upgrade failure, change step S7 over to;

Step S65 upgrades the adaptive value of particle in each described subgroup, the individual optimum solution of particle and the globally optimal solution of subgroup;

Step S66 judges whether the evolution cycle that reaches default, does not then directly change step S67 over to if reach described evolution cycle; Otherwise upgrade described subgroup and described gene pool, upgrade described population center of mass point then, change step S67 again over to;

Step S67, judge whether the maximum iteration time that reaches default, if reached described maximum iteration time, termination of iterations then, obtain the local endpoint of this planning, and described local endpoint is added described terminal point concentrate, as the current point of planning next time, change described local endpoint over to step S2; If do not reach described maximum iteration time, then change step S64 over to and continue iteration;

Step S7 keeps away the barrier failure and handles, and changes described robot over to next GOTO field, returns step S61 and keeps away barrier; If described robot all keeps away the barrier failure at all GOTO fields, then can't realize keeping away barrier, failure is jumped out.

2. the robot path planning method based on coevolution population rolling optimization according to claim 1 is characterized in that, the described working environment modeling of described step S1 specifically comprises the steps:

Step S11 ignores the elevation information in the described working environment, sets up two-dimensional coordinate system;

Step S12, the view field of establishing described robot for (r, θ), wherein r is radius of view, θ is field-of-view angle; Obtain the starting point S of described robot and the coordinate of impact point G; With convex polygon described barrier is described, coordinate the unknown of described barrier;

Step S13 carries out expandedly to described barrier according to the size of described robot, and described robot handled as a particle;

Step S14, it is 1 that current scrolling planning number of times t is set, and current some P of described robot is set ^tBe described starting point S, and with described current some P ^tDeposit terminal point collection { P} in.

3. the robot path planning method based on coevolution population rolling optimization according to claim 2 is characterized in that, described step S3 is specially:

Described current optimal movement orientation angle Refer to current motion towards with the angle of described two-dimensional coordinate system x axle, described current motion is oriented described current some P ^tRectilinear direction to described impact point G; Described current optimal movement orientation angle

Computing formula as the formula (1):

(1)

Wherein, t is current scrolling planning number of times, if

Then order

G _xBe the coordinate figure of described impact point G at the x axle, G _yBe the coordinate figure of described impact point G at the y axle,

Be described current some P ^tAt the coordinate figure of x axle,

Be described current some P ^tCoordinate figure at the y axle.

4. the robot path planning method based on coevolution population rolling optimization according to claim 3 is characterized in that, described step S4 is specially:

According to described field-of-view angle θ, the GOTO field of described robot is divided into K=2 π/θ, according to described current optimal movement orientation angle

Determine the current motion of described robot towards the GOTO field k at place, described current motion towards the computing formula of the GOTO field k at place as the formula (2):

(2)

Wherein, if k=0 then makes k=K, wherein ceil () is the function that rounds up, and mod () is for getting complementary function.

5. the robot path planning method based on coevolution population rolling optimization according to claim 4 is characterized in that, described step S5 is specially:

Described local endpoint P ^t' computing formula as the formula (3):

(3)

Wherein, P ^t' _xBe described local endpoint P ^t' at the coordinate figure of x axle, P ^t' _yBe described local endpoint P ^t' at the coordinate figure of y axle, P _x ^tBe described current some P ^tAt the coordinate figure of x axle, P _y ^tBe described current some P ^tCoordinate figure at the y axle; With described local endpoint P ^t' deposit described terminal point collection in and { behind the P}, make t=t+1.

6. the robot path planning method based on coevolution population rolling optimization according to claim 5 is characterized in that, described step S61 further comprises the steps:

Step S611 is divided into the m section with described radius of view r, obtains a segmentation radius λ, determines m described segment movement orientation angle at random

Wherein,

I=1,2 ... m obtains some segmented paths dist _i, by described some segmented paths dist _iConnect to form described robot and in described radius of view r scope, avoid the path length overall dist that described barrier need pass through _t

Step S612 arranges the interative computation that some particles are used for carrying out particle cluster algorithm, and described particle is encoded to by some described segment movement orientation angles

The sequence of forming, the dimension of establishing described some particles is m, according to the GOTO field k of described current motion towards the place, the scope that defines described some particles is [(k-1) θ, k θ], wherein, and k=1,2 ... K, K=2 π/θ;

Step S613 determines described path length overall dist _t, described path length overall dist _tComputing formula as the formula (4):

${\mathrm{dist}}_t=\mathrm{\&lambda;}+\underset{i-2}{\overset{m}{\mathrm{\&Sigma;}}}{\mathrm{dist}}_i=r/m+\underset{i=2}{\overset{m}{\mathrm{\&Sigma;}}}\sqrt{{\left((i-1)\mathrm{\&lambda;}\right)}^2+{\left(\mathrm{i\&lambda;}\right)}^2+2\mathrm{i\&lambda;}(i-1)\mathrm{\&lambda;}\cos (x_i-x_{i-1})}$ (4)

Wherein, i=2,3 ..., m; λ=r/m, namely radius of view r makes the section length behind the branches such as m; x _iI dimension for the back particle of encoding;

Obtain described local endpoint P ^t' to the air line distance of described impact point G

Described air line distance

Computing formula as the formula (5):

${\mathrm{dist}}_{P^{t\&prime;}G}=\sqrt{{({P^{t\&prime;}}_x-G_x)}^2+{\left({P^{t\&prime;}}_y{-G}_y\right)}^2}$ (5)

According to described path length overall dist _tWith described air line distance

Set an adaptive value function, described adaptive value function as the formula (6):

$\mathrm{Fit}={\mathrm{\&omega;}}_1{\mathrm{dist}}_t+{\mathrm{\&omega;}}_2{\mathrm{dist}}_{P^{t\&prime;}G}$ (6)

ω wherein ₁With ω ₂Be weight coefficient;

Step S614 forms an initial population with described some particles, and initial population is carried out initialization; Described initialization requires current some P from this planning ^tBeginning is with described segmented paths dist _iFor there not being barrier in the view field of radius of view, otherwise the initialization failure;

Step S615 if the initialization success is divided into s subgroup with described initial population, according to the GOTO field k of described current motion towards the place, defines the scope [x of described some particles _Min, x _Max] be [(k-1) θ, k θ]; The maximal rate that particle is set is 0.1 times of particle range.

7. the robot path planning method based on coevolution population rolling optimization according to claim 6 is characterized in that, the initialization among the described step S614 is specially:

The setting original dimensions is that m, initial scale are N, carry out initialization before, the population scale of described initial population is set much larger than initial scale N; The maximum initialization times p of particle is set _MaxWith the maximum initialization times d of dimension _MaxIf certain one dimension of a certain particle is being made d in the described initial population _MaxSuccess not yet then reinitializes current dimension after the inferior initialization; If this particle is through p _MaxInferior initialization is still failed, and then thinks current particle initialization failure;

Abandon the particle of initialization failure, the initialized particle of success is formed described initial population; If the scale of described initial population reaches initial scale N, then initialization success; Otherwise, the initialization failure.

8. the robot path planning method based on coevolution population rolling optimization according to claim 7 is characterized in that, the P of population center of mass point described in the described step S63 _cDefinition as the formula (7):

$P_c(R+1)=\frac{1}{s}\underset{1}{\overset{s}{\mathrm{\&Sigma;}}}{p}_{\mathrm{gd}}^i\left(R\right)i=\mathrm{1,2},...,s$ (7)

Wherein, R is evolution cycle,

It is the globally optimal solution of i subgroup.

9. the robot path planning method based on coevolution population rolling optimization according to claim 8 is characterized in that, described step S64 is specially:

Upgrade particle's velocity and position in each described subgroup according to formula (8) and formula (9):

${v_i}^j(\mathrm{iter}+1)=\mathrm{\&omega;}\&times;{v_i}^j\left(\mathrm{iter}\right)+c_1{r}_1\&times;({P_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_2{r}_2\&times;({g_i}^j\left(\mathrm{iter}\right)-{x_i}^j\left(\mathrm{iter}\right))+c_3{r}_3({P_c}^R-{x_i}^j\left(\mathrm{iter}\right))$

(8)

$x_i^j(\mathrm{iter}+1)=x_i^j\left(\mathrm{iter}\right)+v_i^j(\mathrm{iter}+1)$ (9)

Wherein iter is the population evolutionary generation, and j is the subgroup mark, and i is particle mark in the subgroup,

Be i particle's velocity in j the subgroup; P _i ^jIt is the historical optimum solution of i particle in j the subgroup;

Optimum solution for subgroup, current particle place; c ₁And c ₂Be the study factor; c ₃For exchanging the factor; r ₁, r ₂, r ₃Be the random number between [0,1]; ω is inertia weight, and described inertia weight ω adopts the linear decrease strategy, as the formula (10):

ω(iter)=ω _max-iter×(ω _max-ω _min)/iter _max (10)

Particle's velocity and location boundary processing are comprised: if the d of a certain particle i dimension speed among the j of subgroup $v_{i,j}^d>v_{\max },$ Then order $v_{i,j}^d=v_{\max };$ If $v_{i,j}^d<v_{\min },$ Then order $v_{i,j}^d>v_{\min },$ If $x_{i,j}^d>x_{\max },$ Then order $x_{i,j}^d=x_{\max };$ If $x_{i,j}^d<x_{\min },$ Then $x_{i,j}^d=x_{\min };$

Each dimension after the particle position renewal is required to comprise: from described current some P ^tBeginning is with described segmented paths dist _iFor there not being barrier in the view field of radius of view, if there is then current dimension renewal failure of barrier; If certain one dimension of a certain particle is made d _MaxInferior renewal is success not yet, and then the current dimension of this particle is upgraded failure, if a certain particle is made p _MaxInferior renewal is success not yet, and then this particle upgrades failure; If all subgroups all upgrade failure, termination of iterations and change step S7 over to then.

10. the robot path planning method based on coevolution population rolling optimization according to claim 9 is characterized in that, described step S66 is specially:

Judge whether the evolution cycle R that reaches default, if reach then at first according to default dead ratio, from described gene pool, select particle to substitute the most bad particle in each described subgroup at random; If the adaptive value of a certain particle is less than the adaptive value of a certain particle in the described gene pool in each described subgroup, then replace corresponding particle in the described gene pool with this particle in each described subgroup; Final updating population barycenter P _c

11. the robot path planning method based on coevolution population rolling optimization according to claim 10 is characterized in that, the method that among the described step S7 described robot is changed over to next GOTO field is: if described current optimal movement orientation angle The local endpoint P of this Rolling Planning ^t' be positioned at current some P ^tThe y axle increase to, then each GOTO field is rotated counterclockwise test, otherwise each GOTO field is made the dextrorotation transfer to test.

Images