CN106568432A

CN106568432A - Moving robot primary pose obtaining method and system

Info

Publication number: CN106568432A
Application number: CN201610922713.2A
Authority: CN
Inventors: 杨再甫; 张小*; 张小�; 章征贵
Original assignee: Shanghai View Technologies Co Ltd
Current assignee: Jiangsu Xumeite Environmental Protection Technology Co Ltd
Priority date: 2016-10-20
Filing date: 2016-10-20
Publication date: 2017-04-19
Anticipated expiration: 2036-10-20
Also published as: CN106568432B

Abstract

The invention discloses a moving robot primary pose obtaining method and system. The method comprises the following steps: step 100, obtaining primary information; step 200, according to the primary information, adopting a particle swarm algorithm to obtain the optimal matching rate under a current orientation angle and position; step 300, judging whether the optimal matching rate is greater or equal to a matching threshold value, if the result is positive, executing step 500, if the result is negative, executing step 400; step 400, rotating a robot for a certain angle on the same place, obtaining the optimal matching rate under a certain orientation angle and position after rotation, judging whether the optimal matching rate is greater or equal to the matching threshold value, if the result is positive, executing step 500; step 500, obtaining the position and orientation angle of particles corresponding to the optimal matching rate, and obtaining the primary pose of the moving robot; if the result is negative, executing step 600; and step 600, judging that the moving robot primary pose obtaining is failure. The method has the advantages of quick convergence, high precision, and high particle utilization rate. The moving robot rotates in the same place, the information of environment 360 degrees around the moving robot can be captured, and the information captured by a laser radar becomes more abundant.

Description

A kind of initial pose acquisition methods of mobile robot and system

Technical field

The present invention relates to a kind of mobile robot autonomous navigation field, the initial pose of more particularly to a kind of mobile robot Acquisition methods and system.

Background technology

With the continuous propulsion of industry 4.0, intelligent mobile robot has gradually applied to all trades and professions, has especially cleaned Field.Mobile robot widely uses the requirement intelligent to mobile robot also more and more higher, and mobile robot is initial The accurate acquisition of pose is to ensure its intelligentized crucial ring.

Laser radar is to launch the radar system of the characteristic quantities such as the position of detecting laser beam target, speed.It is former from work Say there is no basic difference with microwave radar in reason：To objective emission detectable signal (laser beam), then will receive from mesh The signal (target echo) that mark is reflected is compared with transmission signal, after making proper treatment, so that it may obtain the relevant of target Information, such as target range, orientation, height, speed, even attitude, shape parameter, so as to carry out to targets such as aircraft, guided missiles Detection, tracking and identification.Infrared and visible light wave range is operated in, the radar with laser as working beam is referred to as laser radar. It is made up of laser transmitter, optical receiver, turntable and information processing system etc., and electric pulse is become light pulse and sent out by laser instrument It is shot out, photoreceiver is reduced into electric pulse and is sent to display the light pulse returned from target reflection again.

Environmental modeling and the basic problem that positioning is Mobile Robotics Navigation field, are also that mobile robot is really realized certainly One of main essential condition, so-called environmental modeling (Mapping) is to set up the various objects of mobile robot institute working environment such as The accurate locus description of obstacle, road sign etc., as spatial model or map.And position (Localization) and then mean Mobile robot to must determine from the exact position in the working environment.Accurate environmental map and mobile robot are fixed Position contributes to efficiently path planning and decision-making, is the basis for ensureing mobile robot safety navigation.

SLAM (Simultaneous Localization And Mapping), Chinese claims " synchronously positioning and build figure ", is The current hot research topic in terms of localization for Mobile Robot, mobile robot sets up environmental map while positioning, its Ultimate principle is, with the method for probability statistics, positioning to be reached by multiple features matching and position error is reduced.Map Representation substantially has 3 kinds, including grid is represented, geometric properties are represented and topological diagram is represented.

At present, the initial pose acquisition methods of existing mobile robot mainly have two kinds：One kind is directly by range finding sensing The swept molding method that device is obtained is matched with the contrast of existing map, and this method is not only computationally intensive, and if environment is varied slightly The algorithm may fail；Another kind of method is that the characteristic information in extraction environment is positioned again with existing map match, this side Characteristic information in method institute extraction environment is still very big, does not have substantial improvement in amount of calculation, and if in space Characteristic information will substantially not cause error ratio larger, positioning precision it cannot be guaranteed that；Yet another method is using particle filter side Method is matched with the contrast of existing map, and this method causes the environmental map set up not enrich.

For the problem that presently, there are, the invention provides a kind of mobile robot initial position fix method and system comes Solve the above problems.

The content of the invention

The present invention there is provided a kind of initial pose acquisition methods of mobile robot and system, optimized using particle cluster algorithm Particle filter calculates Optimum Matching degree, and this algorithm is easily achieved, high precision, convergence are fast；Solved using the strategy for rotating in place The problem that local environmental information is not enriched, after mobile robot rotation secondary or multiple matching is carried out, the rate that improves that the match is successful and Adaptability to environment.

The technical scheme that the present invention is provided is as follows：

The present invention provides a kind of mobile robot initial pose acquisition methods, including：S100 obtains initial information, including ring Border cartographic information and mobile robot towards angular position information；S200 according to the initial information, using particle cluster algorithm Obtain the presently described Optimum Matching degree towards under angle position；S300 judges the presently described optimum towards under angle position Whether it is more than or equal to matching threshold with degree, if then execution step S500；Otherwise, execution step S400；S400 rotates in place one Determine angle, obtain after the rotation of the mobile robot towards the Optimum Matching degree under angle position, judge court after the rotation Whether the Optimum Matching degree under angle position is more than or equal to matching threshold, if then execution step S500；Otherwise, execution step S600；S500 obtains the position and orientation angle of Optimum Matching degree correspondence particle, obtains the initial pose of the mobile robot； The initial pose of S600 mobile robots obtains failure.

The present invention is calculated using the particle filter method of particle cluster algorithm optimization and obtains Optimum Matching degree, with convergence it is fast, High precision, the advantage for improving particle utilization rate, are conducive to improving stability and the intellectuality of intelligent mobile robot.It is simultaneously mobile Robot is rotated in place, and can capture the environmental information in the range of 360 ° of mobile robot so that the information that laser radar is obtained is more Plus it is abundant.Initial pose can be accurately obtained, independent navigation can be better achieved, also can avoid colliding, be to move machine One of safety guarantee of people.

Further, step S400 includes：S410 issues the first rotation instruction, controls the mobile robot rotation； S420 judges whether encounter barrier in rotary course, if then execution step S440；Otherwise, execution step S430；S430 sentences Whether the mobile robot of breaking rotates to the first predetermined angle；If then execution step S431；Otherwise, return to step S410； S431 obtains the mobile robot and rotates Optimum Matching degree of first predetermined angle towards under；S432 judges that described first presets Whether Optimum Matching degree of the angle towards under is more than or equal to matching threshold, if then execution step S500；Otherwise, execution step S600；Mobile robot is stopped the rotation described in S440, obtains optimum of the angle towards under after the mobile robot is stopped the rotation Matching degree；Towards the Optimum Matching degree under angle position whether more than or equal to matching threshold after stopping the rotation described in S441 judgements, If then execution step S500；Otherwise, execution step S450；S450 controls the mobile robot rotation and is back to initial shape State；S460 issues the second rotation instruction, controls phase of the mobile robot relative to the described first rotation instruction direction of rotation Opposite direction is rotated；S461 judges whether the mobile robot rotates to the second predetermined angle, if then execution step S462；Otherwise, return to step S460；S462 obtains the mobile robot and rotates Optimum Matching of second predetermined angle towards under Degree；Whether S463 judges Optimum Matching degree of second predetermined angle towards under more than or equal to matching threshold, if then performing Step S500；Otherwise, execution step S600.

Because the laser scanning angle of laser radar is 180 ° in the present invention, if controlling mobile robot without barrier 180 ° of rotate counterclockwise is first carried out, if barrier so calculates a matching degree, if it is so just not anti-to reach matching threshold To rotation, return after original state if being not reaching to matching threshold and so controlling mobile robot, then reversely rotate i.e. clockwise Rotation, it is possible to obtain the environmental information in the range of 360 ° of mobile robot so that the information that laser radar is obtained more is enriched, from And the initial pose of mobile robot can be accurately obtained, and independent navigation can be better achieved, also can avoid colliding, ensure The safety of mobile robot.

Further, the rotation instruction includes：Rotational speed command, direction of rotation instruction and rotation angle commands, it is described The anglec of rotation is 0-180 ° of angular range.

Further, step S100 includes：S110 emission detection signals, and receive the reflection reflected from barrier Signal；S120 obtains the Environmental Map Information at mobile robot place according to the detectable signal and transmission signal；S130 reads Mobile robot on the environmental map towards angular position information；S140 sends the initial information, including environment ground Figure information and mobile robot towards angular position information.

In the present invention, the laser range of laser radar is 20m, and laser scanning angle is 180 °, can using laser radar To obtain high angle, distance and velocity resolution, and active jamming rejection ability is strong and small volume, the advantage of light weight, So that mobile robot is obtained in that the environmental map of image clearly.

Further, step S200 includes：S210 carries out initialization computing to particle colony, turning according to state variable Move the multiple particles of probability density function sampling；S220 gathers the initial information of the mobile robot；S230 is according to described Initial information, obtains itself fitness value of each particle, and contrast obtains particle individuality history optimal adaptation angle value and population Body history optimal adaptation angle value；S240 obtains the weight of each particle；S250 contrasts obtain optimal weights, and the speed of more new particle Degree and position；Particle rapidities and position of the S260 according to the renewal, updates the weight of each particle；S270 is to all particles Weight is normalized computing；S280 re-starts sampling, and the weight of each particle after resampling is obtained again；

S290 contrasts obtain the optimal weights after resampling and export as Optimum Matching degree.

The probability density function of utilization state variable gives up the less particle of weight ratio in the present invention, reduce weight compared with Few number of particles, can be effectively prevented from the diverging of degradation phenomena and particle collection, so that obtaining more accurate optimum The position and orientation angle of matching degree and particle, can accurately obtain the initial pose of mobile robot, can be better achieved certainly Leading boat.

Further, step S230 includes：S231 obtains itself fitness of each particle using the initial information Value；Whether S232 judges itself fitness value of each particle more than or equal to particle individuality history optimal adaptation angle value, if then Execution step S233, otherwise, execution step S234；S233 is optimal according to the current location more new particle individuality history of the particle Fitness value；S234 keeps particle individuality history optimal adaptation angle value constant；S235 judges the particle individuality history of each particle Whether optimal adaptation angle value is more than or equal to particle colony history optimal adaptation angle value, if then execution step S236, otherwise, performs Step S237；S236 updates particle colony history optimal adaptation angle value according to the current location of the particle；S237 keeps particle Colony's history optimal adaptation angle value is constant.

The present invention is by comparing the increment of the fitness value or the iterationses of particle cluster algorithm described in comparison, if fitted The increment for answering angle value is more than maximum iteration time less than predetermined threshold value and/or particle cluster algorithm, then stop algorithm computing.Pass through Iterative learning more new particle individuality history optimal adaptation angle value and particle colony history optimal adaptation angle value so that quickly obtain The optimal weights of each particle, so as to obtain Optimum Matching degree, can be effectively prevented from the diverging of degradation phenomena and particle collection, tool There is good robustness, so that the position and orientation angle of the more accurate Optimum Matching degree of acquisition and particle, Neng Goujing The initial pose of mobile robot is really obtained, independent navigation can be better achieved.

Further, step S230 obtains fitness value according to formula (1),

Wherein, f represents fitness value, m_iRepresent existing map datum, m '_iRepresent measurement map datum, u_iRepresent using clothes From the measurement noise variance of Gauss distribution.

Further, step S240 obtains the weight of each particle according to formula (2),

Wherein, at the i=0 moment, N number of particle of sampling, the particle for obtaining is usedRepresent, the initial value of weight is 1/N,The weight of i moment particles is represented,The weight of i-1 moment particles is represented,Expression state The probability density function of variable, k represents k-th particle,

Further, step S250 updates speed and the position of each particle according to formula (3),

Wherein,It is the speed of i-1 moment particles,The position of moment particle is represented,Represent i-1 moment grains The position of son, p_pbestItself fitness value and the fitness value of particle individuality history optimal adaptation angle value for representing each particle enters Row compares the particle individuality history optimal adaptation angle value of acquisition, p_gbestRepresent particle individuality history optimal adaptation angle value p_pbestWith Particle colony optimal adaptation angle value is compared the particle colony history optimal adaptation angle value of acquisition, and k represents k-th particle, a and B is the positive random number of Gaussian distributed.

Further, step S270 is normalized computing according to formula (4) to the weight of all particles,

Wherein,The weight of i moment particles is represented, k represents k-th particle, and N represents sampling number of particles.

The present invention provides a kind of initial pose of mobile robot and obtains system, including pedestal, the laser being arranged on pedestal Positioner, also includes：Central processor and rotating control assembly；

The laser locating apparatus are connected with central processor communication, obtain the initial information of mobile robot, Including Environmental Map Information and mobile robot towards angular position information, the initial information is sent to central authorities and processes dress Put；

The central processor is connected with rotating control assembly communication, receives what the laser locating apparatus sent The initial information, using particle cluster algorithm the presently described Optimum Matching degree towards under angle position is obtained；Judge current institute The Optimum Matching degree towards under angle position is stated whether more than or equal to matching threshold, if then obtaining Optimum Matching degree correspondence particle Position and orientation angle, obtain the initial pose of the mobile robot；Rotation instruction is otherwise sent to the Spin Control Device, obtains the mobile robot and rotates in place towards the Optimum Matching degree under angle position after certain angle, judges described Whether it is more than or equal to matching threshold towards the Optimum Matching degree under angle position after rotation, if then obtaining Optimum Matching degree correspondence The position and orientation angle of particle, obtains the initial pose of the mobile robot；The initial bit of otherwise described mobile robot Appearance obtains failure；

The rotating control assembly receives the rotation instruction that the central processor sends, and is referred to according to the rotation The order control mobile robot rotates in place certain angle.

The present invention is calculated using the particle filter method of particle cluster algorithm optimization and obtains Optimum Matching degree, with convergence it is fast, High precision, the advantage for improving particle utilization rate, are conducive to improving stability and the intellectuality of intelligent mobile robot, while mobile Robot is rotated in place, and can capture the environmental information in the range of 360 ° of mobile robot so that the information that laser radar is obtained is more Plus it is abundant.Initial pose can be accurately obtained, independent navigation can be better achieved, also can avoid colliding, be to move machine One of safety guarantee of people.

Further, the central processor also includes：Instruction release module, acquisition module and judge module；

Instruction release module and the rotating control assembly communication be connected, issues the first rotation and instructs to described Rotating control assembly, controls the mobile robot rotation until rotating to the first predetermined angle；Receive the judge module to send out The collision information for sending, issue is stopped the rotation to instruct to the rotating control assembly control mobile robot and is stopped the rotation, and is sent out Cloth return is instructed to the rotating control assembly control mobile robot rotation and is back to original state；Issue second to rotate Instruct to rotating control assembly control, control the mobile robot relative to the described first rotation instruction direction of rotation Rightabout carries out rotation until rotating to the second predetermined angle；

The acquisition module is connected with the instruction release module communication, and using particle cluster algorithm presently described direction is obtained Optimum Matching degree under angle position, sends described current towards the Optimum Matching degree under angle position to the judge module； Instruct when the mobile robot rotates according to the instruction release module is issued described first, stop the rotation instruction, return After instruction and the second rotation instruction control mobile robot start to rotate in place, the mobile robot rotation first is obtained Optimum Matching degree of the predetermined angle towards under, the mobile robot are encountered after barrier stops the rotation towards under angle position Optimum Matching degree and the mobile robot rotate Optimum Matching degree of second predetermined angle towards under；And send the moving machine The rotation relevant information of device people and presently described Optimum Matching degree, the first predetermined angle towards under angle position towards under most Excellent matching degree, stop the rotation after Optimum Matching degree towards the Optimum Matching degree under angle position, the second predetermined angle towards under To the judge module；

The judge module is connected with acquisition module communication, receives the described current direction that the acquisition module sends Optimum Matching degree under angle position, judges described currently towards the Optimum Matching degree under angle position whether more than or equal to matching Threshold value, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, obtains the initial bit of the mobile robot Appearance；Otherwise, the first rotation instruction that the acquisition module sends is received, the mobile robot rotation is controlled, rotation is judged Whether encounter barrier during turning, judge whether the mobile robot rotates to first and preset if it will not hit on barrier Angle, if not rotating to first predetermined angle, sends again rotation information to the instruction release module, according to the finger The first rotation instruction control mobile robot that release module sends is made to rotate to the first predetermined angle；According to described Optimum Matching degree of first predetermined angle that acquisition module sends towards under, judges first predetermined angle towards under Whether Optimum Matching degree is more than or equal to matching threshold, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, The initial pose of the mobile robot is obtained, the otherwise initial pose of mobile robot obtains failure；If barrier is encountered The collision information is sent to the instruction release module, according to the rotation instruction, the return instruction control mobile machine People stops the rotation and controls the mobile robot rotation and is back to original state；Stop according to the acquisition module sends Towards the Optimum Matching degree under angle position after spin-ended turn, towards the Optimum Matching under angle position after stopping the rotation described in judgement Whether degree is more than or equal to matching threshold, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, obtains described The initial pose of mobile robot；The second rotation instruction that the acquisition module sends otherwise is received, the movement is controlled Robot rotates；Judge whether the mobile robot rotates to the second predetermined angle, if not rotating to second preset angle Degree, then send again rotation information to the instruction release module, according to second rotation that the instruction release module sends The instruction control mobile robot is rotated to the second predetermined angle；According to second preset angle that the acquisition module sends Whether Optimum Matching degree of the degree towards under, judge Optimum Matching degree of second predetermined angle towards under more than or equal to matching threshold Value, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, obtains the initial pose of the mobile robot, Otherwise the initial pose of mobile robot obtains failure.

The present invention is by iterative learning more new particle individuality history optimal adaptation angle value and particle colony history optimal adaptation Angle value so that quickly obtain the optimal weights of each particle, so as to obtain Optimum Matching degree, can be effectively prevented from degenerating now As the diverging with particle collection, with good robustness, the particle filter method optimized using particle cluster algorithm is calculated and obtained most Excellent matching degree, has the advantages that fast convergence, high precision, improves particle utilization rate, is conducive to improving the steady of intelligent mobile robot Qualitative and intellectuality, while mobile robot is rotated in place, can capture the environmental information in the range of 360 ° of mobile robot so that The information that laser radar is obtained more is enriched, and can accurately obtain initial pose, and independent navigation can be better achieved, and also can avoid Collide, be one of safety guarantee of mobile robot.

Further, the laser locating apparatus include：Generating laser, optical receiver and message handler；

The generating laser is connected with optical receiver communication, the laser transmitter projects detectable signal；

The optical receiver is connected with the communication of described information processor, receives the reflection reflected from barrier and believes Number；

Described information processor receives the reflected signal, obtain Environmental Map Information that the mobile robot is located and On the environmental map towards angular position information, by the initial information, including Environmental Map Information and mobile machine People's sends to the central processor towards angular position information.

Further, the acquisition module also includes：Collection submodule, computing submodule, resampling submodule and output Module；

The collection submodule is connected with judge module communication, initialization computing is carried out to particle colony, according to shape The multiple particles of probability density function sampling of state variable；Gather the initial information of the mobile robot；

The computing submodule is connected with the collection submodule communication, according to the initial information, obtains each particle Itself fitness value, contrast obtains particle individuality history optimal adaptation angle value and particle colony history optimal adaptation angle value；Obtain Obtain the weight of each particle；Contrast obtains optimal weights, and speed and the position of more new particle；According to the speed of the more new particle Degree and position, update the weight of each particle；Computing is normalized to the weight of all particles；

The resampling submodule is connected with the operator module communication, re-starts sampling, and resampling is obtained again The weight of each particle afterwards；

The output sub-module is connected with resampling submodule communication, and contrast obtains the optimal weights after resampling and makees For the output of Optimum Matching degree.

Further, the computing submodule also includes：Acquiring unit and comparing unit；

The acquiring unit is connected with the collection submodule communication, and using the initial information oneself of each particle is obtained Body fitness value；

The comparing unit is connected with acquiring unit communication, and whether itself fitness value for judging each particle is more than Equal to the fitness value of particle individuality history optimal adaptation angle value, if then according to the current location more new particle of the particle Body history optimal adaptation angle value；Otherwise, keep particle individuality history optimal adaptation angle value constant；Judge the particle of each particle Whether body history optimal adaptation angle value is more than or equal to particle colony history optimal adaptation angle value, if then working as according to the particle Front position updates particle colony history optimal adaptation angle value；Otherwise, keep particle colony history optimal adaptation angle value constant.

The present invention is by iterative learning more new particle individuality history optimal adaptation angle value and particle colony history optimal adaptation Angle value so that quickly obtain the optimal weights of each particle, so as to obtain Optimum Matching degree, can be effectively prevented from degenerating now As the diverging with particle collection, with good robustness, so that the position of the more accurate Optimum Matching degree of acquisition and particle Put and towards angle, can accurately obtain the initial pose of mobile robot, independent navigation can be better achieved.

Further, the computing submodule obtains fitness value according to formula (1),

Further, the computing submodule obtains the weight of each particle according to formula (2),

Further, the computing submodule updates speed and the position of each particle according to formula (3),

Further, the computing submodule is normalized computing according to formula (4) to the weight of all particles,

Compared with prior art, the present invention provides a kind of initial pose acquisition methods of mobile robot and system, at least band Carry out a kind of following technique effect：

1st, the accurate acquisition of the initial pose of mobile robot, can be better achieved independent navigation, also can avoid colliding, It is one of safety guarantee of mobile robot.

2nd, the particle filter algorithm of particle group optimizing has the advantages that fast convergence, high precision, improves particle utilization rate, favorably In the stability and intellectuality that improve intelligent mobile robot.

3rd, mobile robot is rotated in place, and can capture the environmental information in the range of 360 ° of mobile robot so that laser thunder More enrich up to the information for obtaining.

Description of the drawings

Below by clearly understandable mode, preferred implementation is described with reference to the drawings, to a kind of batch testing of chip Method and system characteristic, technical characteristic, advantage and its implementation are further described.

Fig. 1 is a kind of flow chart of one embodiment of the initial pose acquisition methods of mobile robot of the invention；

Fig. 2 is a kind of flow chart of another embodiment of the initial pose acquisition methods of mobile robot of the invention；

Fig. 3 is a kind of flow chart of another embodiment of the initial pose acquisition methods of mobile robot of the invention；

Fig. 4 is a kind of flow chart of the further embodiment of the initial pose acquisition methods of mobile robot of the invention；

Fig. 5 is a kind of flow chart of the further embodiment of the initial pose acquisition methods of mobile robot of the invention；

Fig. 6 is the structure chart of one embodiment that a kind of initial pose of mobile robot of the invention obtains system；

Fig. 7 is the structure chart of another embodiment that a kind of initial pose of mobile robot of the invention obtains system；

Fig. 8 is that a kind of initial pose of mobile robot of the invention is obtained before the initial pose acquisition positioning of system mobile robot Effect diagram afterwards.

Drawing reference numeral explanation

1 pedestal；

101 existing Environmental Map Informations；

102 mobile robots；

103 obtain Environmental Map Information；

1000 laser locating apparatus；

1100 generating lasers；

1200 optical receivers；

1300 message handlers；

2000 central processoies；

2100 instruction release modules；

2200 acquisition modules；

2210 collection submodules；

2220 computing submodules；

2221 acquiring units；

2222 comparing units；

2230 resampling submodules；

2240 output sub-modules；

2300 judge modules；

3000 rotating control assemblies.

Specific embodiment

In order to be illustrated more clearly that the embodiment of the present invention or technical scheme of the prior art, below by control description of the drawings The specific embodiment of the present invention.It should be evident that drawings in the following description are only some embodiments of the present invention, for For those of ordinary skill in the art, on the premise of not paying creative work, can be obtaining other according to these accompanying drawings Accompanying drawing, and obtain other embodiments.

To make simplified form, part related to the present invention is only schematically show in each figure, they are not represented Its practical structures as product.In addition, so that simplified form is readily appreciated, with identical structure or function in some figures Part, only symbolically depicts one of those, or has only marked one of those.Herein, " one " is not only represented " only this ", it is also possible to represent the situation of " more than one ".

The present invention provides a kind of one embodiment of the initial pose acquisition methods of mobile robot, with reference to shown in Fig. 1, bag Include：S100 obtains initial information, including Environmental Map Information and mobile robot towards angular position information；S200 is according to institute Initial information is stated, the presently described Optimum Matching degree towards under angle position is obtained using particle cluster algorithm；S300 judges current Whether the Optimum Matching degree towards under angle position is more than or equal to matching threshold, if then execution step S500；Otherwise, hold Row step S400；S400 rotates in place certain angle, after the rotation of the acquisition mobile robot towards under angle position most Whether excellent matching degree, judge after the rotation towards the Optimum Matching degree under angle position more than or equal to matching threshold, if then Execution step S500；Otherwise, execution step S600；S500 obtains the position and orientation angle of Optimum Matching degree correspondence particle, obtains To the initial pose of the mobile robot；The initial pose of S600 mobile robots obtains failure.

Specifically, the external environmental information and the fortune of itself of mobile robot are obtained using SLAM algorithms by laser radar Dynamic attitude, obtains Environmental Map Information (representation of the environmental map is grating map) and movement that mobile robot is located Robot towards angular position information, according to above-mentioned Environmental Map Information and towards angular position information, using grain of the present invention The particle filter method of swarm optimization optimization is calculated and obtains the presently described Optimum Matching degree towards under angle position, judges current Whether the Optimum Matching degree towards under angle position is more than or equal to matching threshold, and the empirical value of the matching threshold of the present invention is 0.7, if the presently described Optimum Matching degree towards under angle position is more than or equal to 0.7, obtain Optimum Matching degree correspondence grain The position and orientation angle of son, obtains the initial pose of the mobile robot.If presently described towards under angle position Optimum Matching degree is less than 0.7, then mobile robot rotates to an angle in situ, after being rotated towards under angle position most Whether excellent matching degree, judge after rotation towards the Optimum Matching degree under angle position more than or equal to 0.7, if towards angle after rotation Whether the Optimum Matching degree under degree position is more than or equal to 0.7, then obtain the position and orientation angle of Optimum Matching degree correspondence particle, The initial pose of the mobile robot is obtained, the otherwise initial pose of mobile robot obtains failure.

The present invention provides a kind of another embodiment of the initial pose acquisition methods of mobile robot, with reference to shown in Fig. 2.Phase For one embodiment, identical step will not be described here, and step S400 is further included：S410 issues the first rotation and refers to Order, controls the mobile robot rotation；S420 judges whether encounter barrier in rotary course, if then execution step S440；Otherwise, execution step S430；S430 judges whether the mobile robot rotates to the first predetermined angle；If then holding Row step S431；Otherwise, return to step S410；S431 obtains the mobile robot and rotates the first predetermined angle towards under most Excellent matching degree；Whether S432 judges Optimum Matching degree of first predetermined angle towards under more than or equal to matching threshold, if Then execution step S500；Otherwise, execution step S600；Mobile robot is stopped the rotation described in S440, obtains the mobile machine People stop the rotation after Optimum Matching degree of the angle towards under；Towards the optimum under angle position after stopping the rotation described in S441 judgements Whether matching degree is more than or equal to matching threshold, if then execution step S500；Otherwise, execution step S450；S450 controls are described Mobile robot rotation is back to original state；S460 issues the second rotation instruction, controls the mobile robot relative to institute The rightabout for stating the first rotation instruction direction of rotation is rotated；S461 judges whether the mobile robot rotates to second Predetermined angle, if then execution step S462；Otherwise, return to step S460；S462 obtains the mobile robot rotation second Optimum Matching degree of the predetermined angle towards under；S463 judges whether Optimum Matching degree of second predetermined angle towards under is more than Equal to matching threshold, if then execution step S500；Otherwise, execution step S600.

Specifically, the present embodiment is that step S400 is further refined on the basis of one embodiment；With reference to Fig. 2 It is shown；In the present invention, if it is determined that when the presently described Optimum Matching degree towards under angle position is less than 0.7, then first The first rotation instruction is issued, control mobile robot carries out rotate counterclockwise, judges from resting state with speed V=0.2m/s Whether encounter barrier during rotate counterclockwise.

If will not hit on barrier, then be judged as whether mobile robot rotates to the first predetermined angle, if do not had Rotate to the first predetermined angle, then till control mobile robot rotates to always the first predetermined angle；If rotated to First predetermined angle, then obtain the mobile robot and rotate Optimum Matching degree of first predetermined angle towards under, and judges institute Optimum Matching degree of first predetermined angle towards under is stated whether more than or equal to matching threshold, if the first predetermined angle is towards under Optimum Matching degree is more than or equal to matching threshold, then obtains the position and orientation angle of Optimum Matching degree correspondence particle, obtains institute The initial pose of mobile robot is stated, the otherwise initial pose of mobile robot obtains failure.

If encountering barrier, then control mobile robot is stopped the rotation, obtain mobile robot and stop the rotation relief angle Whether Optimum Matching degree of the degree towards under, judgement is more than or equal to matching after stopping the rotation towards the Optimum Matching degree under angle position Threshold value, if being more than or equal to matching threshold towards the Optimum Matching degree under angle position after stopping the rotation, then obtain optimum Position and orientation angle with degree correspondence particle, obtains the initial pose of the mobile robot, otherwise issues the second rotation and refers to Order, the rightabout for controlling mobile robot relative to the first rotation instruction direction of rotation is rotated, that is, control mobile machine People is turned clockwise with speed V=0.2m/s, judges whether mobile robot rotates to the second predetermined angle, if not revolving Go to the second predetermined angle, then till control mobile robot rotates to always the second predetermined angle；If it is pre- to rotate to second If angle, then obtain mobile robot and rotate Optimum Matching degree of second predetermined angle towards under, judge second preset angle Whether Optimum Matching degree of the degree towards under is more than or equal to matching threshold, if Optimum Matching degree of second predetermined angle towards under is big In equal to matching threshold, then obtain the position and orientation angle of Optimum Matching degree correspondence particle, obtain the mobile robot Initial pose, otherwise mobile robot initial pose obtain failure.

In the present invention because the laser scanning angle of laser radar is 180 °, if without the mobile machine of barrier control People first carries out 180 ° of rotate counterclockwise, if barrier so calculates a matching degree, if reaching matching threshold so just not Reversely rotate, return after original state if being not reaching to matching threshold and so controlling mobile robot, then reversely rotate i.e. up time Pin rotates, it is possible to obtain the environmental information in the range of 360 ° of mobile robot so that the information that laser radar is obtained more is enriched, So as to the accurate initial pose for obtaining mobile robot, independent navigation can be better achieved, also can avoid colliding, be protected The safety of barrier mobile robot.

The present invention provides a kind of another embodiment of the initial pose acquisition methods of mobile robot, with reference to shown in Fig. 3.Phase For one embodiment, identical step will not be described here, and step S100 is further included：S110 emission detection signals, And the reflected signal that reception is reflected from barrier；S120 obtains mobile robot according to the detectable signal and transmission signal The Environmental Map Information at place；S130 read mobile robot on the environmental map towards angular position information；S140 Send the initial information, including Environmental Map Information and mobile robot towards angular position information.

Specifically, the present embodiment is that step S100 is further refined on the basis of one embodiment；With reference to Fig. 3 It is shown；In the present invention, the laser range of laser radar is 20m, and laser scanning angle is 180 °, can be obtained using laser radar High angle, distance and velocity resolution, and active jamming rejection ability is strong and small volume, the advantage of light weight so that Mobile robot is obtained in that the environmental map of image clearly.

The present invention provides a kind of another embodiment of the initial pose acquisition methods of mobile robot, with reference to shown in Fig. 4.Phase For one embodiment, identical step will not be described here, and step S200 is further included：S210 is carried out to particle colony Initialization computing, according to the multiple particles of the probability density function of state variable sampling；S220 gathers the mobile robot The initial information；S230 obtains itself fitness value of each particle according to the initial information, and contrast obtains particle Body history optimal adaptation angle value and particle colony history optimal adaptation angle value；S240 obtains the weight of each particle；S250 is contrasted Obtain optimal weights, and speed and the position of more new particle；Particle rapidities and position of the S260 according to the renewal, updates each The weight of particle；S270 is normalized computing to the weight of all particles；S280 re-starts sampling, and resampling is obtained again The weight of each particle afterwards；

Specifically, the present embodiment is that step S200 is further refined on the basis of one embodiment；With reference to Fig. 4 It is shown；In the present invention, according to the probability density function of state variable 2000 particles of sampling, at the i=0 moment, sample N Individual particle, the particle for obtaining is usedRepresent, the initial value of weight is 1/N, initialization computing is carried out to particle colony, according to institute State collection initial information, obtain itself fitness value of each particle, contrast obtain particle individuality history optimal adaptation angle value and Particle colony history optimal adaptation angle value, obtain the weight of each particle, carries out contrast and obtains optimal weights, and more new particle Speed and position, the particle rapidity and position according to the renewal, update the weight of each particle, and the weight of all particles is entered Row normalization computing, re-starts sampling, gives up the less particle of weight ratio, and the power of each particle after resampling is obtained again Weight, contrast obtains the optimal weights after resampling and exports as Optimum Matching degree, and exports the corresponding particle of Optimum Matching degree Position and orientation angle.

Step S230 obtains fitness value according to formula (1),

Step S240 obtains the weight of each particle according to formula (2),

Step S250 updates speed and the position of each particle according to formula (3),

Step S270 is normalized computing according to formula (4) to the weight of all particles,

The present invention provides a kind of another embodiment of the initial pose acquisition methods of mobile robot, with reference to shown in Fig. 5.Phase For upper one embodiment, identical step will not be described here, and step S230 is further included：S231 utilizes the initial letter Breath obtains itself fitness value of each particle；Whether S232 judges itself fitness value of each particle more than or equal to particle Body history optimal adaptation angle value, if then execution step S233, otherwise, execution step S234；S233 working as according to the particle Front position more new particle individuality history optimal adaptation angle value；S234 keeps particle individuality history optimal adaptation angle value constant；S235 Whether the particle individuality history optimal adaptation angle value of each particle is judged more than or equal to particle colony history optimal adaptation angle value, if It is then execution step S236, otherwise, execution step S237；S236 updates particle colony history according to the current location of the particle Optimal adaptation angle value；S237 keeps particle colony history optimal adaptation angle value constant.

Specifically, the present embodiment is that step S230 is further refined on the basis of upper one embodiment；With reference to Fig. 4 It is shown；By iterative learning more new particle individuality history optimal adaptation angle value and particle colony history optimal adaptation angle value so that The optimal weights of each particle are quickly obtained, so as to obtain Optimum Matching degree, degradation phenomena and particle can be effectively prevented from The diverging of collection, with good robustness, so that the position and orientation of the more accurate Optimum Matching degree of acquisition and particle Angle, can accurately obtain the initial pose of mobile robot, and independent navigation can be better achieved.

The present invention is by comparing the increment of the fitness value or the iterationses of particle cluster algorithm described in comparison, if fitted The increment for answering angle value is more than maximum iteration time less than predetermined threshold value and/or particle cluster algorithm, then stop algorithm computing.

The present invention provides one embodiment that a kind of initial pose of mobile robot obtains system, with reference to shown in Fig. 6, bag Include：Pedestal 1, the laser locating apparatus 1000 being arranged on pedestal 1, central processor 2000 and rotating control assembly 3000； The communication of the laser locating apparatus 1000 and the central processor 2000 is connected, the central processor 2000 with it is described The communication connection of rotating control assembly 3000.

Specifically, the laser locating apparatus 1000 are generally laser radar, and the laser range of laser radar is 20m, is swashed Optical scan angle be 180 °, by laser radar using SLAM algorithms obtain mobile robot external environmental information and itself Athletic posture, obtains Environmental Map Information (representation of the environmental map is grating map) and shifting that mobile robot is located Mobile robot towards angular position information, send to central authorities' process by the Environmental Map Information and towards angular position information Device 2000, central processor 2000 receives the Environmental Map Information and direction of the transmission of the laser locating apparatus 1000 Angular position information, the particle filter method optimized using particle cluster algorithm of the present invention is calculated and obtains presently described towards angle position Whether Optimum Matching degree under putting, judge the presently described Optimum Matching degree towards under angle position more than or equal to matching threshold, The empirical value of the matching threshold of the present invention is 0.7, if the presently described Optimum Matching degree towards under angle position is more than or equal to 0.7, then the position and orientation angle of Optimum Matching degree correspondence particle is obtained, obtain the initial pose of the mobile robot.Such as The really presently described Optimum Matching degree towards under angle position is less than 0.7, then send rotation instruction to the rotating control assembly 3000 control mobile robots rotate to an angle in situ, towards the Optimum Matching degree under angle position after being rotated, sentence Whether it is more than or equal to 0.7 towards the Optimum Matching degree under angle position after disconnected rotation, if after rotation towards under angle position Whether Optimum Matching degree is more than or equal to 0.7, then obtain the position and orientation angle of Optimum Matching degree correspondence particle, obtains the shifting The initial pose of the initial pose of mobile robot, otherwise mobile robot obtains failure.The rotating control assembly 3000 is received The rotation instruction that the central processor 2000 sends, according to the rotation instruction control mobile robot original place Rotate to an angle.

The present invention provides another embodiment that a kind of initial pose of mobile robot obtains system, with reference to shown in Fig. 7.Phase For upper one embodiment, identical part will not be described here, and the central processor 2000 also includes：Mould is issued in instruction Block 2100, acquisition module 2200 and judge module 2300；

The instruction release module 2100 is connected with the communication of the rotating control assembly 3000,

The acquisition module 2200 is connected with instruction release module 2100 communication, and the acquisition module 2200 is also wrapped Include：Collection submodule 2210, computing submodule 2220, resampling submodule 2230 and output sub-module 2240；Collection Module 2210 is connected with the communication of the judge module 2300, and the computing submodule 2220 is communicated with the collection submodule 2210 Connection, the computing submodule 2220 also includes：Acquiring unit 2221 and comparing unit 2222；The acquiring unit 2221 and institute The collection communication connection of submodule 2210 is stated, the resampling submodule 2230 is connected with the computing submodule 2220 communication, institute State the communication of output sub-module 2240 and the resampling submodule 2230 to be connected, the comparing unit 2222 and the acquiring unit 2221 communication connections,

The judge module 2300 is connected with the communication of the acquisition module 2200；

The laser locating apparatus 1000 include：Generating laser 1100, optical receiver 1200 and message handler 1300；

The generating laser 1100 is connected with the communication of the optical receiver 1200, and the generating laser 1100 is launched Detectable signal；The optical receiver 1200 is connected with the communication of described information processor 1300, and reception is reflected from barrier Reflected signal；

Specifically, the present embodiment is that central processor 2000 and laser positioning are filled on the basis of upper one embodiment 1000 further refinements are put, with reference to shown in Fig. 7, with reference to Fig. 8, by taking one section of T-shaped corridor as an example, (a) figure represents that initial pose is obtained Effect diagram before taking, (b) figure represents that initial pose obtains the effect diagram after success, wherein 101 refer to existing environment Cartographic information, 102 refer to mobile robot, including position and orientation, and 103 refer to acquisition Environmental Map Information, are obtained by laser radar 's.

Instruction release module 2100 issues the first rotation instruction to the rotating control assembly 3000, controls the shifting Mobile robot rotation is until rotate to the first predetermined angle；The collision information of the transmission of the judge module 2300 is received, issue stops Spin-ended turn instructs and controls the mobile robot to the rotating control assembly 3000 and stop the rotation, and issue back bit instruction is to described Rotating control assembly 3000 controls the mobile robot rotation and is back to original state；The second rotation instruction is issued to the rotation Rotation control apparatus 3000 are controlled, and control rightabout of the mobile robot relative to the described first rotation instruction direction of rotation Rotation is carried out until rotating to the second predetermined angle；

Acquisition module 2200 obtains the presently described Optimum Matching degree towards under angle position using particle cluster algorithm, sends It is described current towards the Optimum Matching degree under angle position to the judge module 2300；When the mobile robot is according to described The the first rotation instruction issued of instruction release module 2100, instruction of stopping the rotation, return instruction and the second rotation instruction control Make after the mobile robot starts to rotate in place, obtain the mobile robot and rotate optimum of first predetermined angle towards under Matching degree, the mobile robot are encountered after barrier stops the rotation towards the Optimum Matching degree under angle position and the movement Robot rotates Optimum Matching degree of second predetermined angle towards under；And send the mobile robot rotation relevant information and Optimum Matching degree of presently described Optimum Matching degree, the first predetermined angle towards under angle position towards under, stop the rotation after Optimum Matching degree of the Optimum Matching degree, the second predetermined angle towards under angle position towards under is to the judge module 2300；

The judge module 2300 receive the transmission of the acquisition module 2200 it is described current towards under angle position most Excellent matching degree, judges described currently towards the Optimum Matching degree under angle position whether more than or equal to matching threshold, if then obtaining The position and orientation angle of Optimum Matching degree correspondence particle is taken, the initial pose of the mobile robot is obtained；Otherwise, institute is received State the first rotation instruction of the transmission of acquisition module 2200, control mobile robot rotation, judge be in rotary course It is no to encounter barrier, judge whether the mobile robot rotates to the first predetermined angle if it will not hit on barrier, if not First predetermined angle is rotated to, then sends again rotation information to the instruction release module 2100, sent out according to the instruction The first rotation instruction control mobile robot that cloth module 2100 sends is rotated to the first predetermined angle；According to described Optimum Matching degree of first predetermined angle that acquisition module 2200 sends towards under, judges the first predetermined angle direction Under Optimum Matching degree whether be more than or equal to matching threshold, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle Degree, obtains the initial pose of the mobile robot, and the otherwise initial pose of mobile robot obtains failure；If encountering barrier The collision information is then sent to the instruction release module 2100, according to the rotation instruction, the return instruction control shifting Mobile robot stops the rotation and controls the mobile robot rotation and is back to original state；According to the acquisition module 2200 Towards the Optimum Matching degree under angle position after stopping the rotation described in sending, after stopping the rotation described in judgement towards under angle position Optimum Matching degree whether be more than or equal to matching threshold, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle Degree, obtains the initial pose of the mobile robot；Second rotation for otherwise receiving the transmission of the acquisition module 2200 refers to Order, controls the mobile robot rotation；Judge whether the mobile robot rotates to the second predetermined angle, if not rotating to Second predetermined angle, then send again rotation information to the instruction release module 2100, according to the instruction release module 2100 the second rotation instruction control mobile robots for sending are rotated to the second predetermined angle；According to the acquisition mould Optimum Matching degree of second predetermined angle that block 2200 sends towards under, judges second predetermined angle towards under most Whether excellent matching degree is more than or equal to matching threshold, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, obtains To the initial pose of the mobile robot, the otherwise initial pose acquisition failure of mobile robot.

The emission detection signal of generating laser 1100 to surrounding, optical receiver 1200 receive the barrier from surrounding Hinder the reflected signal that thing is reflected；Message handler 1300 receives reflected signal, obtains the environment ground that mobile robot is located Figure information and on the environmental map towards angular position information, by the initial information, including Environmental Map Information and Mobile robot is sent to the central processor 2000 towards angular position information.

The acquisition module 2200 also includes：Collection submodule 2210, computing submodule 2220, resampling submodule 2230 With output sub-module 2240；The collection submodule 2210 is connected with the communication of the judge module 2300, and particle colony is carried out Initialization computing, according to the multiple particles of the probability density function of state variable sampling；Gather the institute of the mobile robot State initial information；The computing submodule 2220 obtains itself fitness value of each particle according to the initial information, contrast Obtain particle individuality history optimal adaptation angle value and particle colony history optimal adaptation angle value；Obtain the weight of each particle；It is right Than obtaining optimal weights, and speed and the position of more new particle；Speed and position according to the more new particle, updates each grain The weight of son；Computing is normalized to the weight of all particles；

The resampling submodule 2230 re-starts sampling, and the weight of each particle after resampling is obtained again；It is described The contrast of output sub-module 2240 obtains the optimal weights after resampling and exports as Optimum Matching degree.

The computing submodule 2220 also includes：Acquiring unit 2221 and comparing unit 2222；

The acquiring unit 2221 obtains itself fitness value of each particle using the initial information；

Whether more than or equal to particle individuality history most the comparing unit 2222 judges itself fitness value of each particle The fitness value of good fitness value, if then according to the current location more new particle individuality history optimal adaptation degree of the particle Value；Otherwise, keep particle individuality history optimal adaptation angle value constant；Judge the particle individuality history optimal adaptation degree of each particle Whether value is more than or equal to particle colony history optimal adaptation angle value, if then updating population according to the current location of the particle Body history optimal adaptation angle value；Otherwise, keep particle colony history optimal adaptation angle value constant.

Computing submodule 2220 obtains fitness value according to formula (1),

Computing submodule 2220 obtains the weight of each particle according to formula (2),

Computing submodule 2220 updates speed and the position of each particle according to formula (3),

Computing submodule 2220 is normalized computing according to formula (4) to the weight of all particles,

It should be noted that above-described embodiment can independent assortment as needed.The above is only the preferred of the present invention Embodiment, it is noted that for those skilled in the art, in the premise without departing from the principle of the invention Under, some improvements and modifications can also be made, these improvements and modifications also should be regarded as protection scope of the present invention.

Claims

1. initial pose acquisition methods of a kind of mobile robot, it is characterised in that including step：

S100 obtains initial information, including Environmental Map Information and mobile robot towards angular position information；

S200 obtains the presently described Optimum Matching towards under angle position according to the initial information using particle cluster algorithm Degree；

Whether S300 judges the presently described Optimum Matching degree towards under angle position more than or equal to matching threshold, if then performing Step S500；Otherwise, execution step S400；

S400 rotates in place certain angle, obtains after the rotation of the mobile robot towards the Optimum Matching under angle position Whether degree, judge after the rotation towards the Optimum Matching degree under angle position more than or equal to matching threshold, if then performing step Rapid S500；Otherwise, execution step S600；

S500 obtains the position and orientation angle of Optimum Matching degree correspondence particle, obtains the initial pose of the mobile robot；

The initial pose of S600 mobile robots obtains failure.

2. initial pose acquisition methods of mobile robot according to claim 1, it is characterised in that step S400 is also Including step：

S410 issues the first rotation instruction, controls the mobile robot rotation；

S420 judges whether encounter barrier in rotary course, if then execution step S440；Otherwise, execution step S430；

S430 judges whether the mobile robot rotates to the first predetermined angle；If then execution step S431；Otherwise, return Step S410；

S431 obtains the mobile robot and rotates Optimum Matching degree of first predetermined angle towards under；

Whether S432 judges Optimum Matching degree of first predetermined angle towards under more than or equal to matching threshold, if then performing Step S500；Otherwise, execution step S600；

Mobile robot is stopped the rotation described in S440, obtains optimum of the angle towards under after the mobile robot is stopped the rotation With degree；

Towards the Optimum Matching degree under angle position whether more than or equal to matching threshold after stopping the rotation described in S441 judgements, if Then execution step S500；Otherwise, execution step S450；

S450 controls the mobile robot rotation and is back to original state；

S460 issues the second rotation instruction, controls phase of the mobile robot relative to the described first rotation instruction direction of rotation Opposite direction is rotated；

S461 judges whether the mobile robot rotates to the second predetermined angle, if then execution step S462；Otherwise, return Step S460；

S462 obtains the mobile robot and rotates Optimum Matching degree of second predetermined angle towards under；

Whether S463 judges Optimum Matching degree of second predetermined angle towards under more than or equal to matching threshold, if then performing Step S500；Otherwise, execution step S600.

3. initial pose acquisition methods of mobile robot according to claim 2, it is characterised in that：The rotation instruction bag Rotational speed command, direction of rotation instruction and rotation angle commands are included, the anglec of rotation is 0-180 ° of angular range.

4. initial pose acquisition methods of mobile robot according to claim 1, it is characterised in that step S100 is also Including step：

S110 emission detection signals, and receive the reflected signal reflected from barrier；

S120 obtains the Environmental Map Information at mobile robot place according to the detectable signal and transmission signal；

S130 read mobile robot on the environmental map towards angular position information；

S140 sends the initial information, including Environmental Map Information and mobile robot towards angular position information.

5. initial pose acquisition methods of mobile robot according to claim 1, it is characterised in that step S200 is also Including step：

S210 carries out initialization computing to particle colony, according to the multiple particles of the probability density function of state variable sampling；

S220 gathers the initial information of the mobile robot；

S230 obtains itself fitness value of each particle according to the initial information, and it is optimal that contrast obtains particle individuality history Fitness value and particle colony history optimal adaptation angle value；

S240 obtains the weight of each particle；

S250 contrasts obtain optimal weights, and speed and the position of more new particle；

Particle rapidities and position of the S260 according to the renewal, updates the weight of each particle；

S270 is normalized computing to the weight of all particles；

S280 re-starts sampling, and the weight of each particle after resampling is obtained again；

6. initial pose acquisition methods of mobile robot according to claim 4, it is characterised in that step S230 is also Including step：

S231 obtains itself fitness value of each particle using the initial information；

Whether S232 judges itself fitness value of each particle more than or equal to particle individuality history optimal adaptation angle value, if then Execution step S233, otherwise, execution step S234；

Current location more new particle individuality history optimal adaptation angle value of the S233 according to the particle；

S234 keeps particle individuality history optimal adaptation angle value constant；

S235 judges whether the particle individuality history optimal adaptation angle value of each particle most preferably fits more than or equal to particle colony history Angle value is answered, if then execution step S236, otherwise, execution step S237；

S236 updates particle colony history optimal adaptation angle value according to the current location of the particle；

S237 keeps particle colony history optimal adaptation angle value constant.

7. the method that the initial pose of mobile robot according to claim 5 is obtained, it is characterised in that：Step S230 Fitness value is obtained according to formula (1),

f = e^{\frac{- 1}{2 u_{i}} {(m_{i} - m_{i}^{'})}^{2}} - - - (1)

Wherein, f represents fitness value, m_iRepresent existing map datum, m '_iRepresent measurement map datum, u_iRepresent high using obeying The measurement noise variance of this distribution.

8. the method that the initial pose of mobile robot according to claim 5 is obtained, it is characterised in that：Step S240 The weight of each particle is obtained according to formula (2),

\begin{matrix} w_{i}^{k} = w_{i - 1}^{k} p (m_{i} | x_{i}^{k}) \\ = w_{i - 1}^{k} e^{\frac{- 1}{2 u_{i}} {(m_{i} - m_{i}^{'})}^{2}} \end{matrix} - - - (2)

Wherein, at the i=0 moment, N number of particle of sampling, the particle for obtaining is usedRepresent, the initial value of weight is 1/N,Table Show the weight of i moment particles,The weight of i-1 moment particles is represented,Represent state variable Probability density function, k represents k-th particle,

9. the method that the initial pose of mobile robot according to claim 5 is obtained, it is characterised in that：Step S250 Speed and the position of each particle are updated according to formula (3),

\begin{matrix} v_{i - 1}^{k} = a * (p_{p b e s t} - x_{i - 1}^{k}) + b * (p_{g b e s t} - x_{i - 1}^{k}) \\ x_{i}^{k} = x_{i - 1}^{k} + v_{i - 1}^{k} \end{matrix} - - - (3)

Wherein,It is the speed of i-1 moment particles,The position of moment particle is represented,Represent the position of i-1 moment particles Put, p_pbestItself fitness value and the fitness value of particle individuality history optimal adaptation angle value for representing each particle is compared The particle individuality history optimal adaptation angle value of acquisition, p_gbestRepresent particle individuality history optimal adaptation angle value p_pbestWith population Body optimal adaptation angle value is compared the particle colony history optimal adaptation angle value of acquisition, and k represents k-th particle, and a and b is clothes From the positive random number of Gauss distribution.

10. the method that the initial pose of mobile robot according to claim 5 is obtained, it is characterised in that：The step S270 is normalized computing according to formula (4) to the weight of all particles,

w_{i}^{k} = \frac{w_{i}^{k}}{Σ_{k = 1}^{N} w_{i}^{k}} - - - (4)

A kind of initial pose of 11. mobile robots obtains system, including pedestal, the laser locating apparatus that are arranged on pedestal it is special Levy and be, also include：Central processor and rotating control assembly；

The laser locating apparatus are connected with central processor communication, obtain the initial information of mobile robot, including Environmental Map Information and mobile robot towards angular position information, the initial information is sent to central processor；

The central processor is connected with rotating control assembly communication, receives the described of the laser locating apparatus transmission Initial information, using particle cluster algorithm the presently described Optimum Matching degree towards under angle position is obtained；Judge presently described court Whether the Optimum Matching degree under angle position is more than or equal to matching threshold, if then obtaining the position of Optimum Matching degree correspondence particle Put and towards angle, obtain the initial pose of the mobile robot；Rotation instruction is otherwise sent to the rotating control assembly, Obtain the mobile robot to rotate in place towards the Optimum Matching degree under angle position after certain angle, after judging the rotation Whether the Optimum Matching degree towards under angle position is more than or equal to matching threshold, if then obtaining Optimum Matching degree correspondence particle Position and orientation angle, obtains the initial pose of the mobile robot；The initial pose of otherwise described mobile robot is obtained Failure；

The rotating control assembly receives the rotation instruction that the central processor sends, according to the rotation instruction control Make the mobile robot and rotate in place certain angle.

The initial pose of 12. mobile robots according to claim 11 obtains system, it is characterised in that the central authorities are processed Device also includes：Instruction release module, acquisition module and judge module；

The instruction release module is connected with rotating control assembly communication, issues the first rotation instruction to the rotation Control device, controls the mobile robot rotation until rotating to the first predetermined angle；Receive what the judge module sent Collision information, issue is stopped the rotation to instruct to the rotating control assembly control mobile robot and is stopped the rotation, issue back Bit instruction to the rotating control assembly controls the mobile robot rotation and is back to original state；Issue the second rotation instruction To rotating control assembly control, the mobile robot is controlled relative to the contrary of the described first rotation instruction direction of rotation Direction carries out rotation until rotating to the second predetermined angle；

The acquisition module is connected with the instruction release module communication, obtains presently described towards angle using particle cluster algorithm Optimum Matching degree under position, sends described current towards the Optimum Matching degree under angle position to the judge module；Work as institute Mobile robot is stated according to the instruction release module the first rotation instruction issued, instruction of stopping the rotation, return instruction After starting to rotate in place with the second rotation instruction control mobile robot, obtain the mobile robot rotation first and preset Optimum Matching degree of the angle towards under, the mobile robot are encountered after barrier stops the rotation towards the optimum under angle position Matching degree and the mobile robot rotate Optimum Matching degree of second predetermined angle towards under；And send the mobile robot Optimum towards under of rotation relevant information and presently described Optimum Matching degree, the first predetermined angle towards under angle position With the Optimum Matching degree after spending, stopping the rotation towards the Optimum Matching degree under angle position, the second predetermined angle towards under to institute State judge module；

The judge module is connected with acquisition module communication, receives the described current towards angle of the acquisition module transmission Optimum Matching degree under position, judges described currently towards the Optimum Matching degree under angle position whether more than or equal to matching threshold Value, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, obtains the initial pose of the mobile robot； Otherwise, the first rotation instruction that the acquisition module sends is received, the mobile robot rotation is controlled, judges to rotate through Whether encounter barrier in journey, judge whether the mobile robot rotates to the first preset angle if it will not hit on barrier Degree, if not rotating to first predetermined angle, sends again rotation information to the instruction release module, according to the instruction The first rotation instruction control mobile robot that release module sends is rotated to the first predetermined angle；Obtained according to described Optimum Matching degree of first predetermined angle that delivery block sends towards under, judges first predetermined angle towards under most Whether excellent matching degree is more than or equal to matching threshold, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, obtains To the initial pose of the mobile robot, the otherwise initial pose acquisition failure of mobile robot；Send out if barrier is encountered The collision information is sent to the instruction release module, according to the rotation instruction, the return instruction control mobile robot Stop the rotation and control the mobile robot rotation and be back to original state；According to the stopping that the acquisition module sends Towards the Optimum Matching degree under angle position after rotation, towards the Optimum Matching degree under angle position after stopping the rotation described in judgement Whether it is more than or equal to matching threshold, if then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, obtains the shifting The initial pose of mobile robot；The second rotation instruction that the acquisition module sends otherwise is received, the moving machine is controlled Device people rotates；Judge whether the mobile robot rotates to the second predetermined angle, if not rotating to second predetermined angle, Rotation information is then sent again to the instruction release module, according to the second rotation instruction that the instruction release module sends Control the mobile robot to rotate to the second predetermined angle；According to the second predetermined angle court that the acquisition module sends Whether downward Optimum Matching degree, judge Optimum Matching degree of second predetermined angle towards under more than or equal to matching threshold, If then obtaining the position and orientation angle of Optimum Matching degree correspondence particle, the initial pose of the mobile robot is obtained, it is no Then the initial pose of mobile robot obtains failure.

The initial pose of 13. mobile robots according to claim 12 obtains system, it is characterised in that：The rotation instruction Including rotational speed command, direction of rotation instruction and rotation angle commands, the anglec of rotation is 0-180 ° of angular range.

The initial pose of 14. mobile robots according to claim 11 obtains system, it is characterised in that the laser positioning Device includes：Generating laser, optical receiver and message handler；

The optical receiver is connected with the communication of described information processor, receives the reflected signal reflected from barrier；

Described information processor receives the reflected signal, obtains the Environmental Map Information at the mobile robot place and in institute State on environmental map towards angular position information, by the initial information, including Environmental Map Information and mobile machine People's sends to the central processor towards angular position information.

The initial pose of 15. mobile robots according to claim 12 obtains system, it is characterised in that the acquisition module Also include：Collection submodule, computing submodule, resampling submodule and output sub-module；

The collection submodule is connected with judge module communication, and to particle colony initialization computing is carried out, and is become according to state The multiple particles of probability density function sampling of amount；Gather the initial information of the mobile robot；

The computing submodule be connected with the collection submodule communication, according to the initial information, obtain each particle oneself Body fitness value, contrast obtains particle individuality history optimal adaptation angle value and particle colony history optimal adaptation angle value；Obtain every The weight of individual particle；Contrast obtains optimal weights, and speed and the position of more new particle；According to the speed of the more new particle and Position, updates the weight of each particle；Computing is normalized to the weight of all particles；

The resampling submodule is connected with the operator module communication, re-starts sampling, obtains every after resampling again The weight of individual particle；

The output sub-module is connected with resampling submodule communication, and contrast obtains the optimal weights after resampling as most Excellent matching degree output.

The initial pose of 16. mobile robots according to claim 15 obtains system, it is characterised in that the computing submodule Block also includes：Acquiring unit and comparing unit；

The acquiring unit is connected with the collection submodule communication, and using the initial information itself fitting for each particle is obtained Answer angle value；

The comparing unit is connected with acquiring unit communication, and whether itself fitness value for judging each particle is more than or equal to The fitness value of particle individuality history optimal adaptation angle value, if then being gone through according to the current location more new particle individuality of the particle History optimal adaptation angle value；Otherwise, keep particle individuality history optimal adaptation angle value constant；The particle individuality for judging each particle is gone through Whether history optimal adaptation angle value is more than or equal to particle colony history optimal adaptation angle value, if then according to the present bit of the particle Put renewal particle colony history optimal adaptation angle value；Otherwise, keep particle colony history optimal adaptation angle value constant.

The initial pose of 17. mobile robots according to claim 15 obtains system, it is characterised in that：The computing submodule Tuber obtains fitness value according to formula (1),

f = e^{\frac{- 1}{2 u_{i}} {(m_{i} - m_{i}^{'})}^{2}} - - - (1)

The initial pose of 18. mobile robots according to claim 15 obtains system, it is characterised in that：The computing submodule Tuber obtains the weight of each particle according to formula (2),

\begin{matrix} w_{i}^{k} = w_{i - 1}^{k} p (m_{i} | x_{i}^{k}) \\ = w_{i - 1}^{k} e^{\frac{- 1}{2 u_{i}} {(m_{i} - m_{i}^{'})}^{2}} \end{matrix} - - - (2)

The initial pose of 19. mobile robots according to claim 15 obtains system, it is characterised in that：The computing submodule Tuber updates speed and the position of each particle according to formula (3),

\begin{matrix} v_{i - 1}^{k} = a * (p_{p b e s t} - x_{i - 1}^{k}) + b * (p_{g b e s t} - x_{i - 1}^{k}) \\ x_{i}^{k} = x_{i - 1}^{k} + v_{i - 1}^{k} \end{matrix} - - - (3)

The initial pose of 20. mobile robots according to claim 15 obtains system, it is characterised in that：The computing submodule Tuber is normalized computing according to formula (4) to the weight of all particles,

w_{i}^{k} = \frac{w_{i}^{k}}{Σ_{k = 1}^{N} w_{i}^{k}} - - - (4)