CN102169344B - Multi-robot cooperative exploring and mapping method with communication distances limited under unknown environments - Google Patents

Abstract

The invention discloses a multi-robot cooperative exploring and mapping method with communication distances limited under unknown environments, belonging to the field of intelligent robot systems. The method comprises the following steps of: (1) planning local target points satisfying the constraint of the communication distances for a plurality of robots; and (2) planning motion trails from the current positions of the robots to the local target points, that is to say, planning trails. The method provided by the invention has the beneficial effects of: not only considering the factor of a linear speed, but also fully considering the influence of an angular speed on a planning time. Under the condition that the initial attitude angles of the robots are not changed, the synchronization of the motion times of the plurality of robots are kept through computing a reasonable linear speed and the motion trails of the robots are computed through solving a differential equation so that the continuous angular speed control input of all the robots is ensured, the extra time caused by the pivot turn of each robot is prevented and the plurality of robots can arrive at the target points at the same time. Through an increment grid clustering method provided by the invention, the clustering property is kept and the computation performance is improved.

Description

Drawing method is explored and built to the limited multi-robot Cooperation of communication distance under the circumstances not known

Technical field

The invention belongs to the intelligent robot system field, relate to the multirobot discovery techniques, can be used for the limited extensive environment of communication distance.

Background technology

The mobile robot can replace the people to carry out the tasks such as exploration, detection and operation in the environment of various complexity or danger owing to having mobility.The mobile robot has progressively moved towards practical through years of researches and development, in traditional field such as manufacturing industry, logistics, service sectors, has had many use mobile robots to enhance productivity or substitutes the example of manual work.And at some the fields such as national economy, society, national defence are had in the project of great strategic significance, mobile robot's demand is also day by day obvious.Continuous expansion along with the mankind's activity scope, the mobile robot is at celestial body detecting, ocean development, military anti-terrorism, disaster relief, huge effect has been given play in the fields such as dangerous material processing gradually, and show widely application prospect, and these fields all exist significantly uncertainty, that have or even fully unknown new environment, exploring surrounding environment becomes mobile robot's basic task, if robot is uncertain at one, finish complex task in the non-structured environment, key is the perception surrounding environment and creates map, and this also is the basis of finishing other tasks.In fact, robot operation under the hazardous environment of being everlasting as searching and rescuing etc., is therefore studied the search problem of Mobile Robots in Unknown Environment in the buildings that caves in, practicality to the raising mobile robot is most important, and widely economy and social value are arranged.

In circumstances not known, the exploration task often requires robot to finish in the short period of time as far as possible fully perception of environment and builds figure, than single robot, multi-robot Cooperation is explored the advantage with more efficient, reliable and robust, therefore has been subject to the extensive concern in mobile robot field.

Multirobot is explored the subject matter that faces and is comprised: 1) how effectively to coordinate the exploratory behaviour of multirobot.If lack coordination, then a plurality of robots may follow the tracks of identical track search environment, cause exploring the result with like the unit device mankind, therefore need to be the different mode of motion of a plurality of Robot Selections, make them can explore the zones of different of environment, realize distributed exploration.2) be difficult to communication owing to communication distance is limited, in extensive environment, multirobot usually can not be at any time, communication everywhere, so information fusion is enjoyed restriction together.

Most heuristic approach guides robot move to nearest zone of ignorance, and is wherein classic algorithm towards having explored the zone and not explored border motion between the zone, and evaluation criterion comprises distance, visibility region etc.In addition, consider from the control architecture angle, multi-robot system can be divided into central type and distributed, and central module of central type maintenance is responsible for assignment decisions; And each robot carries out the local optimum decision-making according to self and neighbours' machine human behavior in the distributed frame.Yet; under extensive environment; might be because distant and be difficult to communication each other between the robot; cause information can not share and fusion at any time, everywhere; and suppose all generally in the existing research that the communication distance between the multirobot satisfies infinitely great; therefore can carry out communication at any time, everywhere, and seldom have research can consider communication distance restricted problem between the multirobot.

Document Jing Yuan, Yalou Huang, Tong Tao, " ACooperative Approach for Multi-Robot Area Exploration " IEEE/RSJInternational Conference on Intelligent Robots and Systems, 2010, Taipei, Taiwan, 1390-1395. the limited multirobot of Research Notes distance is explored, and proposes the strategy that a plurality of robots arrive impact point simultaneously in the literary composition.Yet when planning, movement locus only considered the factor of linear velocity, robot needs the original place to spin to deflect into certain angle just can carry out next step linear velocity input, and the error that the time of this pivot turn and turning are introduced is not considered, cause thus under truth, robot is difficult to arrive simultaneously impact point.

Summary of the invention

The present invention proposes and a kind ofly not only considered the factor of linear velocity but also fully taken into account under the circumstances not known of angular velocity the limited multi-robot Cooperation of communication distance to explore and build drawing method.

The limited multi-robot Cooperation of communication distance is explored and is built drawing method under the circumstances not known of the present invention, may further comprise the steps:

1) be the localized target point that a plurality of robot plannings satisfy the communication distance constraint based on the frontier point of exploring the zone and do not explore between the zone, i.e. localized target point planning;

2) plan by the robot current location to the movement locus the localized target point, i.e. trajectory planning.

Described localized target point planning comprises the steps:

A) frontier point is carried out cluster, and with the class center after the cluster as the candidate target point;

B) take the head of robot towards and itself and candidate target point line between angle, robot and candidate target point between distance as evaluation index, the candidate target point set of calculated population gain maximum is as the localized target point set, and the target point set of planning need satisfy the mutually constraint of communication between the robot.

Described trajectory planning is specially:

The R of robot _iThe trajectory planning implementation step be:

The R of robot _iThe initial value of track vector

Wherein 0 represent initial time 0, Be R _iCurrent coordinate,

Be R _iHead towards with the angle of x axle, i is the robot numbering.

The single order derived vector of track vector after to the time differentiate is:

$y^{\′}=\left[\begin{array}{c}1\\ V_{y\left(5\right)}\·\cos \left(y\left(4\right)\right)\\ V_{y\left(5\right)}\·\sin \left(y\left(4\right)\right)\\ -V_{y\left(5\right)}\·\sin {\mathrm{\α}}_{y\left(5\right)}/\sqrt{{(y\left(2\right)-{\mathrm{Dx}}_i)}^2+{(y\left(3\right)-{\mathrm{Dy}}_i)}^2}\\ 0\end{array}\right]$

V wherein _{Y (5)}Represent the R of robot _iSpeed because this method is insensitive to speed, robot keeps constant at an exploratory stage medium velocity.α _{Y (5)}The R of robot _iHead towards and itself and impact point line between angle; (Dx _i, Dy _i) be R _iThe coordinate of impact point.

By separating the differential equation, obtain the R of robot _iFrom current location to localized target motion of point track.

Yet, most of frontier points are old frontier point and have finished cluster in a lot of situations, if at every turn with this part frontier point again cluster, obviously can cause and calculate waste, step is a) described carries out cluster to frontier point, when most of frontier points are old frontier point and when having finished cluster, adopt a kind of method to increment frontier point cluster, when being each cluster, only consider to finish to frontier point newly-increased the current time and near old frontier point thereof from last cluster, increment frontier point cluster comprises the steps:

A) frontier point of first exploratory stage gained carried out cluster, and cluster result is assigned in the corresponding chest;

B) in the ensuing exploratory stage, for newly-increased frontier point distributes new chest, capped frontier point is removed from the chest at its place, and cluster increases frontier point and near old frontier point thereof newly again, and above-mentioned " chest " refers to be equipped with by classification results the data structure of frontier point.

This method proposes the fixed point communication strategy, be the localized target point that a plurality of robot plannings satisfy the communication distance constraint based on exploring the zone and not exploring frontier point between the zone, and plan that each robot arrives the movement locus of its impact point, make all robots arrive simultaneously separately impact point, carrying out communication and map merges, and when planning robot's impact point and movement locus, taken into full account the coordination between the multirobot, realize distributed exploration.

Environmental map represents with Discrete Grid in the present invention, and robot can calculate at any time the frontier point of exploring the zone and not exploring between the zone and gather.Be that N robot distributes N frontier point, frontier point need satisfy robot and arrive behind this point the mutually condition of communication.Yet, continuous exploration along with robot, the number of frontier point is on the increase, therefore the calculated amount of estimating frontier point increases rapidly, and the performance index of adjacent boundary point that belong to same regional area are usually quite similar, so first with the frontier point cluster, the class center after the cluster is as the candidate target point.As candidate's point set, N robot calculates the Performance Evaluating Indexes of each candidate point with central points all kinds of after the cluster, then calculates the overall assessment index of N robot.The target point set that generates must be limited in the communication context of robot, and makes indicators of overall performance maximum.

Beneficial effect of the present invention is: not only considered the factor of linear velocity but also fully taken into account angular velocity to the impact of planning time.The present invention is not in the situation that change the robot initial attitude angle, keep the synchronous of a plurality of robot motion time by calculating rational linear velocity, and by finding the solution the differential equation, calculating robot's movement locus, the angular velocity control inputs that guarantees all robots is continuous, thereby, avoided each robot pivot turn and extra time of producing, guarantee that a plurality of robots can arrive impact point simultaneously.The increment grid clustering method that the present invention proposes not only keeps the character of cluster but also has improved calculated performance.

Description of drawings

Continuous two exploratory stage candidate target dot generation results of Fig. 1 embodiment 1

The final exploration result of the emulation experiment of Fig. 2 embodiment 1

Embodiment

Embodiment 1

In the present embodiment the exploration task is divided into minute a plurality of stages, the motion of robot between the localized target point of double planning is a stage, T ⁱRepresent i exploratory stage.To the R of robot _iThe j stage represents from j-1 impact point to the motion j the impact point, and first stage refers to the motion between the point from the reference position to the first aim.Each stage initial time need to be planned local impact point, carries out trajectory planning.

The planning of localized target point

Each exploratory stage initial time need to be planned local impact point, is divided into candidate target dot generation and localized target dot generation two parts.

The candidate target dot generation

The candidate target point is the central point after the frontier point cluster.Thereby need some data structures of definition to reach the purpose of increment frontier point cluster to distinguish old border and new border.

Data structure

Grating map

Represent environment with grating map, each grid representing occupied probability situation, is represented free zone with 0 by assignment, 1 expression barrier, and the district is explored in 0.5 expression.Adjacent free grid represents to explore the zone and does not explore border grid between the zone with exploring grid, and border grid central point is called frontier point.Each border grid by assignment with expression cluster classification.In order to represent that with probability phase region divides, the border grid is assigned F+i,, (F＞1, i=1 ... M, M are clusters number) represent that this border grid belongs to the i class.

The cluster chest

The chest that distributes a correspondence for each class cluster point, for example, the chest that classification i is corresponding is: bin[i]={ old_dount, new_count, center, points}, wherein, old_count and new_count represent that respectively original and existing cluster is counted out in the chest, and center is cluster centre, points is the set that belongs to the frontier point of i class, points={point ₁, point ₂

M _iNumber for the frontier point that belongs to the i class.Initial value for each chest: bin[i] .old_count=bin[i] .new_count=0, bin[i] .center and bin[i] .points is sky.

The set of reunion class

There is the frontier point of part classification to need again cluster, the classification numbering is put into reunion class set reclust_set;

Increment grid cluster

In each step motion of each exploratory stage, to explore environment, upgrade simultaneously each data structure.

First exploratory stage

First exploratory stage initial time carries out cluster to frontier point, supposes that clusters number is M.Be the numerical value of its cluster classification of representative with each border grid assignment, namely i class cluster frontier point is assigned F+i.Each chest need to carry out assignment.Bin[i], (i=1 ... M) bin[i in] .new_count and bin[i] .old_count is assigned the number of frontier point in such, bin[i] .center is the cluster centre point, bin[i] .points is the frontier point (comprising central point) in such.

At each exploratory stage T ⁱ, (i＞1) repeats following step until arrive till the localized target point.

(1) robot whenever makes a move needs the probability of the corresponding grid in renewal overlay area to occupy value.The grid that is about to be endowed 0 value need check whether this grid was the border grid originally, if, each such grid is found out category label c=num-F corresponding to this grid, num is grid numerical value.At bin[c] this grid point is rejected among the .points (for search convenient can set points with the tree-like formula storage of kd), upgrade bin[c] .new_count, bin[c] .new_count=bin[c] .new_count-1, if category label c in reunion class set reclust_set, does not put it among the reclust_set.

(2) whenever the make a move new border grid that obtains of robot (can adopt the edge-follow method of image in processing, or the grid that sensor is explored terminal correspondence is judged as the border grid in the zone is explored in the part.) for each new border grid, frontier point is put into bin[M+1] .points, upgrade bin[M+1] .new_count, bin[M+1] .new_count=bin[M+1] .new_count+1, and be F+M+1 with border grid assignment.

When localized target point place arrived localized target point, (being the finish time in this stage, zero hour next stage) checked that whether newly-increased frontier point is more than the old frontier point of having rejected.Rejected old frontier point number and be designated as oldfrontiers_count, computing method are:

The newly-increased frontier point number of oldfrontiers_count=sum (bin[reclust_set] .old_count)-sum (bin[reclust_set] .new_count) is designated as newfrontiers_count, and computing method are newfrontiers_count=bin[M+1] .new_count.Rejecting frontier point if newly-increased frontier point is less than, is bin[1 with all frontier points then ... M+1] .points all participates in cluster, again packs among the bin by the narration of exploratory stage 1 after the cluster; Otherwise the frontier point that participates in cluster is bin[reclust_set] .points and bin[M+1] .points two parts, be called increment frontier point cluster.

Clustering method adopts kmeans or k central point algorithm, and the two all needs to specify in advance clusters number to be difficult to be applicable to the situation that classification changes.This method adopts a kind of adaptive clustering method (subtractive clusting), determine class number by computational data density, and select the highest data point of density value as the initial classes center, for increment frontier point cluster, the class number M ' of cluster part must not less than count (reclust_set)-1, then use K-means or k center point method to carry out cluster.Obtain all kinds of cluster centres and corresponding point after the cluster, center is cluster centre point set, center[k] central point of expression k class, points is cluster point combination, points[k] represent the point in the k class, k=1 ... M '.Result after the cluster is put into corresponding chest, and false code is as follows:

Empty reunion class set reclust_set, cluster sum M=M-count (reclust_set)+M '.

Select 2 dimensional vector P=[x in this method _fy _f] ^TAs cluster feature, (x wherein _f, y _f) expression frontier point coordinate, with (x _f, y _f) be that to make the continuous and close frontier point that is positioned at same regional area poly-be a class as the purpose of feature.Take out the cluster centre point in each chest, be respectively center _j=bin[j] .center, (j=1,2...M) is with D _Candidate={ center _j, j=1,2...M} is as the candidate target point set of each robot.

Fig. 1 has provided the candidate target dot generation result who uses continuous two stages that said method obtains, and the class of meeting again as seen from the figure border only comprises that classification is 3,13,7,17,9 frontier point, participate in the frontier point number of cluster much smaller than all boundary point, thereby improve calculated performance; The distribution results of last cluster still keeps same classification performance index similar, the large character of different classes of performance index difference.

The localized target dot generation

For the R of robot _i, each candidate target point center wherein _jPerformance index be:

W wherein ₁And w ₂Be weight,

Be the R of robot _iAt center _jThe newly-increased area at place,

For robot head towards with robot and center _jBetween the angle of line,

Then the localized target point of N robot planning is:

And satisfy following constraint condition:

$\left\{\begin{array}{c}{J}_i\∈\{J_{i,j},j=\mathrm{1,2}...N\}\\ \left|\right|D_p-D_q\left|\right|\≤\min \{C_p,C_q\},(\∀p,q=\mathrm{1,2}...N)\end{array}.\right.$

Second constraint condition refers to that localized target point that all robot plannings go out should be positioned at the communication context of robot in the following formula.

Obtaining to satisfy makes the candidate target point set of indicators of overall performance maximum as the localized target point.

Trajectory planning

Draw each angular velocity constantly by finding the solution the differential equation, the method can guarantee that angular velocity is continuous, thereby the time difference of having avoided the original place to spin and bring guarantees that robot can arrive impact point simultaneously.Concrete solution procedure is as follows:

λ _iBe the R of robot _iAngle with its impact point line and x axle:

${\mathrm{\&lambda;}}_i=\left\{\begin{array}{cc}\arctan \left(\frac{{\mathrm{Dy}}_i-y_{r_i}}{{\mathrm{Dx}}_i-x_{r_i}}\right)& {\mathrm{Dx}}_i-x_{r_i}>0\\ \mathrm{\&pi;}/2& {\mathrm{Dx}}_i-x_{r_i}=0\\ \arctan \left(\frac{{\mathrm{Dy}}_i-y_{r_i}}{{\mathrm{Dx}}_i-x_{r_i}}\right)+\mathrm{\&pi;}& {\mathrm{Dx}}_i-x_{r_i}<0\end{array}\right.$

Wherein,

Be the R of robot _iPose, (Dx _i, Dy _i) be R _iThe coordinate of impact point.

α _iThe R of robot _iHead towards and itself and impact point line between angle:

% is the modulo operation symbol in the following formula.

Suppose the R of robot ₁Be the maximum robot of localized target point gain.V ₁Be R ₁Linear velocity.Robot linear velocity in heuristic process is constant, angular velocity varies, and each robot arrives impact point simultaneously, can get thus other machines people's linear velocity:

$V_i,(i\&NotEqual;1)=\left\{\begin{array}{cc}\mathrm{Dis}(D_i,i)\&CenterDot;{\mathrm{<figure-callout\; id="1"\; label="V"\; filenames="HSA00000404404600011.png"\; state="\{\{state\}\}">V</figure-callout>}}_1\&CenterDot;\cos \left({\mathrm{\&alpha;}}_1\right)/(\mathrm{Dis}({\mathrm{<figure-callout\; id="1"\; label="D"\; filenames="HSA00000404404600011.png"\; state="\{\{state\}\}">D</figure-callout>}}_1,1)\&CenterDot;\cos \left({\mathrm{\&alpha;}}_i\right))& \left(1\right)\\ -\mathrm{Dis}(D_i,i)\&CenterDot;{\mathrm{<figure-callout\; id="1"\; label="V"\; filenames="HSA00000404404600011.png"\; state="\{\{state\}\}">V</figure-callout>}}_1\&CenterDot;\cos \left({\mathrm{\&alpha;}}_1\right)/(\mathrm{Dis}({\mathrm{<figure-callout\; id="1"\; label="D"\; filenames="HSA00000404404600011.png"\; state="\{\{state\}\}">D</figure-callout>}}_1,1)\&CenterDot;\cos \left({\mathrm{\&alpha;}}_i\right))& \left(2\right)\end{array}\right.,$

(1), (2) are definition α _iThe time two restrictive conditions, wherein,

Be the R of robot _iDistance to its impact point.Condition (2) refers to that robot needs reverse travel to keep angular velocity continuous.

Order:

Wherein 0 represent initial time 0, i is the robot numbering.

$y^{\&prime;}=\left[\begin{array}{c}1\\ V_{y\left(5\right)}\&CenterDot;\cos \left(y\left(4\right)\right)\\ V_{y\left(5\right)}\&CenterDot;\sin \left(y\left(4\right)\right)\\ -V_{y\left(5\right)}\&CenterDot;\sin {\mathrm{\&alpha;}}_{y\left(5\right)}/\sqrt{{(y\left(2\right)-{\mathrm{Dx}}_i)}^2+{(y\left(3\right)-{\mathrm{Dy}}_i)}^2}\\ 0\end{array}\right],$

Find the solution the above-mentioned differential equation, can get the R of robot _iFrom current location to localized target motion of point track:

t _i=Dis (D _i, i)/| V _iCos α _i| be the R of robot _iTo the time of its impact point, t _i=t _j, (i ≠ j).

Utilize the Runge Kutta method to ask [0, t _i] numerical solution of the differential equation in the time range, thereby obtain each angular velocity constantly, namely obtain arriving localized target motion of point track by current location.Fig. 2 has provided the final exploration result of emulation experiment.

Claims (2)

1. the limited multi-robot Cooperation of communication distance is explored and is built drawing method under the circumstances not known, it is characterized in that, may further comprise the steps:

1) be the localized target point that a plurality of robot plannings satisfy the communication distance constraint based on the frontier point of exploring the zone and do not explore between the zone, i.e. localized target point planning;

2) plan by the robot current location to the movement locus the localized target point, i.e. trajectory planning;

Described localized target point planning comprises the steps:

A) frontier point is carried out cluster, and with the class center after the cluster as the candidate target point;

B) take the head of robot towards and itself and candidate target point line between angle, robot and candidate target point between distance as evaluation index, the candidate target point set of calculated population gain maximum is as the localized target point set, and the target point set of planning need satisfy the mutually constraint of communication between the robot;

Described trajectory planning is specially:

The R of robot _iThe trajectory planning implementation step be:

The R of robot _iThe initial value of track vector

Wherein 0 represent initial time 0,

Be R _iCurrent coordinate,

For the head of Ri towards with the angle of x axle, i is the robot numbering;

The single order derived vector of track vector after to the time differentiate is:

$y^{\&prime;}=\left[\begin{array}{c}1\\ V_{y\left(5\right)}\&CenterDot;\cos \left(y\left(4\right)\right)\\ V_{y\left(5\right)}\&CenterDot;\sin \left(y\left(4\right)\right)\\ -V_{y\left(5\right)}\&CenterDot;{\sin \mathrm{\&alpha;}}_{y\left(5\right)}/\sqrt{{(y\left(2\right)-{\mathrm{Dx}}_i)}^2+{(y\left(3\right)-{\mathrm{Dy}}_i)}^2}\\ 0\end{array}\right]$

V wherein _y(5) represent the R of robot _iSpeed because this method is insensitive to speed, robot keeps constant at an exploratory stage medium velocity; α _{Y (5)}The R of robot _iHead towards and itself and impact point line between angle; (Dx _i, Dy _i) be R _iThe coordinate of impact point;

By separating the differential equation, obtain the R of robot _iFrom current location to localized target motion of point track.

2. drawing method is explored and built to the limited multi-robot Cooperation of communication distance under the circumstances not known as claimed in claim 1, it is characterized in that, step is a) described carries out cluster to frontier point, when most of frontier points are old frontier point and when having finished cluster, adopt a kind of method to increment frontier point cluster, when being each cluster, only consider to finish to frontier point newly-increased the current time and near old frontier point thereof from last cluster, increment frontier point cluster comprises the steps:

A) frontier point of first exploratory stage gained carried out cluster, and cluster result is assigned in the corresponding chest;

B) in the ensuing exploratory stage, for newly-increased frontier point distributes new chest, capped frontier point is removed from the chest at its place, and cluster increases frontier point and near old frontier point thereof newly again, and above-mentioned " chest " refers to be equipped with by classification results the data structure of frontier point.

Images