CN102121827B - Positioning system of mobile robot and positioning method thereof - Google Patents

Abstract

The invention discloses a positioning system of a mobile robot and a positioning method thereof. The positioning system comprises a base station. A dead reckoning positioning system comprises an angular rate sensor used for acquiring angle information of the mobile robot and a displacement sensor used for acquiring walking distance information of the mobile robot. An ultrasonic laser positioning system comprises an ultrasonic laser transmitting device arranged on the base station, an ultrasonic laser receiving device arranged on the mobile robot and an information processing system. The position coordinates of the mobile robot in the ultrasonic laser positioning system can be obtained through the information exchange between the ultrasonic laser transmitting device and the ultrasonic laser receiving device. A data fusion unit is used for the fusion of the angle information and the walking distance information in the dead reckoning positioning system as well as the fusion of two position coordinates in the dead reckoning positioning system and the ultrasonic laser positioning system. The elimination of accumulated errors in the dead reckoning positioning system is implemented by the ultrasonic laser positioning system through the data fusion unit.

Description

A kind of mobile robot positioning system and localization method thereof

Technical field

The invention belongs to the mobile positioning technique field of intelligent machine, relate in particular to a kind of mobile robot's positioning system and localization method thereof.

Background technology

Along with the fast development of computer technology, microelectric technique, network technology, mobile robot's gordian technique has obtained more deep research, and part moves to maturity.Mobile robot's working environment has destructuring and uncertainty, thereby also higher to the requirement of robot, location technology wherein is a very crucial technical matters, and the overall situation is located mobile robot's a critical function especially, there is no this function, any autokinetic movement of robot is all blindly.No matter be which kind of robot, it will solve three problems at the volley all the time, " now where? ", " where going to? ", " how going to? ", mobile robot's location, the research of airmanship are exactly in order to solve above-mentioned three problems.

At the existing odometer of positioning field, calculate, the multiple localization methods such as the overall situation location of the landmark identification based on vision, map-based coupling, gyroscopic navigation, GPS, every kind of technology has advantage and limitation separately, although odometer calculates that short-term accuracy is high, cost is low, can not avoid attracting the unlimited accumulation of error; Gyroscopic navigation is without external reference, but drift is arranged in time, is not suitable for for a long time accurately location.For the working environment of mobile robot's the non-structure of the unknown, only have at present GPS could realize can be practical overall situation location, but GPS is subject to the restriction of the factors such as precision, safety.In mobile robot's practical application, be generally all the Integrated using of multinomial location technology, realize that the relative merits complementation is to improve positioning precision and reliability.As China national Patent Office, in the disclosed patent No. of 2010-02-03, be that " 200920157556.6 ", name are called the patent of " a kind of Position Fixing Navigation System of container Automatic Guided Vehicle ", adopt the localization method of inertial navigation, GPS, laser positioning combination, although the location real-time is good, precision is high, but still will rely on GPS to realize.

To sum up, current mobile robot is far from reaching practical requirement, its intelligent various demands that also do not meet the mankind.Those skilled in the art are also making great efforts to attempt, to explore more practical, intelligent robot always, but fail breakthrough development localization for Mobile Robot is technical.

Summary of the invention

Problem to be solved by this invention just is to provide a kind of mobile robot positioning system and localization method thereof, can accurately implement location, accelerates robot response to external world, avoids intricate reasoning, improves system real time and applicability.

For solving the problems of the technologies described above, the present invention adopts following technical scheme:

A kind of mobile robot positioning system is characterized in that: comprise base station;

Be installed on boat position on the mobile robot and infer positioning system, described boat position infers that positioning system comprises angular-rate sensor for obtaining mobile robot's angle information and for obtaining the displacement transducer of mobile robot's travel distance information; Obtain the position coordinates of mobile robot in positioning system is inferred in the boat position after merging angle information and travel distance information;

The ultrasonic laser positioning system, described ultrasonic laser positioning system comprises the ultrasonic laser emitter that is arranged on base station, is arranged on ultrasonic laser receiving trap and information handling system on the mobile robot, by the message exchange between ultrasonic laser emitter and ultrasonic laser receiving trap, obtains the position coordinates of mobile robot in the ultrasonic laser positioning system;

The data fusion unit, the fusion of positioning system angle information and travel distance information and the fusion that two position coordinateses in positioning system and ultrasonic laser positioning system are inferred in the boat position are inferred for the position of navigating in described data fusion unit; Realize the elimination of ultrasonic laser positioning system to cumulative errors in boat position supposition positioning system by the data fusion unit.

Further, described angular-rate sensor is MEMS digital gyro instrument, and described displacement transducer is the incremental encoding code-disc.

Further, described ultrasonic laser emitter comprises that generating laser, correspondence with photoelectric code disk are arranged on the drive unit of ultrasonic transmitter, driving laser transmitter and ultrasonic transmitter synchronous rotary on generating laser; The ultrasonic laser receiving trap comprise laser pick-off array that the laser pickoff by some annular spread forms, by some ultrasonic receivers form and with the ultrasound wave receiving array of the corresponding installation of laser pick-off array.

Further, have eight laser pickoffs in the laser pick-off array at least, have eight ultrasonic receivers in the ultrasound wave receiving array at least, each laser pickoff receiving side signal corresponding to ultrasonic receiver to.Its objective is allow as far as possible the mobile robot multi-faceted receive laser and the ultrasound wave from base station.

Further, described information handling system comprises by laser, ultrasonic signal and triggers the central processing module of external interrupt and for the wireless module of exchange message.

Further, the upper generating laser of installing with photoelectric code disk of described mobile robot, be equipped with the laser pick-off array on base station.The positional information of mobile robot in environment, it is also important state towards angle.Gyroscope measured towards there being random drift, for this reason, a generating laser with code-disc of same installation on the mobile robot, a laser pick-off array is installed on base station, be used for measuring base station residing angle in mobile robot's coordinate system, in conjunction with mobile robot's coordinate, just can calculate the mobile robot towards angle.

Further, described ultrasonic laser positioning system also comprises laser transmission circuit, the ultrasonic transmit circuit for promoting the ultrasound wave emissive power, second signal amplifying circuit and the phase discriminator for regulating the Laser emission frequency.Adjusting by laser transmission circuit makes scan laser on scan laser on the mobile robot and base station have no transmission frequency, mutually interference-free, also make the laser pick-off array receive only the laser of characteristic frequency, be not subject to the interference of external environment light simultaneously; By ultrasonic transmit circuit, can make hyperacoustic measuring distance reach more than 20 meters, increase mobile robot's sphere of action; And second signal amplifying circuit and phase discriminator realize receiving on laser pickoff amplification processing and the phase wave demodulation of signal, guarantee laser and hyperacoustic receiving range.

Further, described data fusion unit is Kalman filter, by the Kalman filtering strategy, the position data of ultrasonic laser positioning system and boat position is inferred to the position data that positioning system obtains is merged.

In order further to solve the problems of the technologies described above, the invention allows for a kind of method for positioning mobile robot, it is characterized in that comprising the steps:

1) angular-rate sensor in boat position supposition positioning system and displacement transducer obtain respectively the mobile robot angle information and travel distance information; And angle information and travel distance information exchange are crossed to the fusion of data fusion unit, obtain the position coordinates that mobile robot in positioning system is inferred in the boat position;

2) the ultrasonic laser emitter on base station and the ultrasonic laser receiving trap on the mobile robot complete message exchange by information handling system, obtain the position coordinates of mobile robot in the ultrasonic laser positioning system;

3) in the position coordinates that mobile robot in positioning system is inferred in boat position and ultrasonic laser positioning system, mobile robot's position coordinates merges by the data fusion unit, eliminates the cumulative errors in boat position supposition positioning system.

Further, also comprise the correction of mobile robot towards angle, the steps include: that the generating laser on the mobile robot sends scan laser scanning base station; Laser pick-off array received on base station is informed the mobile robot by wireless module after scan laser; By the photoelectric code disk on the mobile robot obtain scan laser angle and and then be converted into the angle information of mobile robot in base station coordinates system; This angle information and boat position are inferred to the angle information that in positioning system, angular-rate sensor obtains is merged, eliminate the cumulative errors of angular-rate sensor.

Further, the message exchange step of described information handling system is:

1) generating laser on base station sends scan laser, triggers central processing module when the laser pick-off array received on the mobile robot arrives this scan laser and produces external interrupt XINT1, and send marking signal by wireless module to base station;

2), after base station receives marking signal, by being arranged on the angle information of photoelectric code disk on base station, by wireless module, be sent to the mobile robot;

3) mobile robot triggers central processing module generation external interrupt XINT13 after receiving angle information, and records time of reception;

4) ultrasonic transmitter on base station sends ultrasound wave, and the ultrasound wave receiving array on the mobile robot triggers central processing module generation external interrupt XINT2 after receiving ultrasound wave, and records time of reception.

Further, the boat position infers that the position coordinates of mobile robot in positioning system obtains by following formula:

$x\left(k\right)=x(k-1)+\mathrm{\Δx}$

$\≈x(k-1)+\mathrm{\Δs}\·\cos (\mathrm{\θ}(k-1)+\frac{\mathrm{\Δ\θ}}{2})$

$=x(k-1)+\mathrm{\Δs}\·\cos \left(\frac{\mathrm{\θ}(k-1)+(\mathrm{\θ}(k-1)+\mathrm{\Δ\θ})}{2}\right);$

$=x(k-1)+\mathrm{\Δs}\·\cos \left(\frac{\mathrm{\θ}(k-1)+\mathrm{\θ}\left(k\right)}{2}\right)$

$y\left(k\right)=y(k-1)+\mathrm{\Δy}$

$\≈y(k-1)+\mathrm{\Δs}\·\sin (\mathrm{\θ}(k-1)+\frac{\mathrm{\Δ\θ}}{2})$

$=y(k-1)+\mathrm{\Δs}\·\sin \left(\frac{\mathrm{\θ}(k-1)+(\mathrm{\θ}(k-1)+\mathrm{\Δ\θ})}{2}\right);$

$=y(k-1)+\mathrm{\Δs}\·\sin \left(\frac{\mathrm{\θ}(k-1)+\mathrm{\θ}\left(k\right)}{2}\right)$

Δ x wherein, Δ y, Δ θ mean respectively the mobile robot in a cycle period from (x (k-1), y (k-1), the recruitment of horizontal ordinate x, ordinate y and angle θ while θ (k-1)) putting to (x (k), y (k), θ (k)); Δ s means mobile robot's path that point is passed by from (x (k-1), y (k-1), θ (k-1)) point to (x (k), y (k), θ (k));

In the ultrasonic laser positioning system, mobile robot's position coordinates obtains by following formula:

X=d _rbcos (α); Y=d _rbsin (α); D wherein _rbmean that the mobile robot is to the distance between base station; α means on base station the angle-data that photoelectric code disk records;

Above-mentioned boat position is inferred to the position of mobile robot coordinate that positioning system is calculated constantly at k is made as (x _d(k), y _d(k) position coordinates that), the ultrasonic laser positioning system is calculated is made as (x _a, y _a), the position coordinates of two systems is merged by following formula:

x(k)＝x _d(k)+k _x(x _a-x _d(k))；y(k)＝y _d(k)+k _y(y _a-y _d(k))；

K wherein _x, k _ymean Error Gain, value is at 0.5-1.0; Then make (x _d(k), y _d(k))=(x (k), y (k)), eliminate the boat position and infer the cumulative errors in positioning system.

Beneficial effect of the present invention:

1, the designed positioning system of the present invention is for the mobile robot, and a multi-sensor information that adopts kalman filter method to infer ultrasonic laser and boat position is merged, thereby has obtained the wide novel localization method of applicability;

2, ultrasonic laser and boat position are inferred to two kinds of traditional localization method combinations, both eliminated in time the cumulative errors in the inertial positioning, guaranteed again real-time and the accuracy of location, boat position;

3, in conjunction with relative positioning and absolute fix mode, widened the requirement of mobile work robot environment, no matter be indoor or outdoor, all without artificial landmark and working environment map, only need a base station to get final product;

4, by the application of ultrasonic laser positioning system, significantly improved mobile robot's positioning precision, also extended mobile robot's working time, be conducive to improve mobile robot's operation performance simultaneously;

5, the mobile robot can auto-initiation in any position of positioning system effective range of the present invention.

To sum up, positioning system provided by the present invention can accurately be implemented location, accelerates robot response to external world, avoids intricate reasoning, and real-time and the applicability of positioning system significantly improve.

The accompanying drawing explanation

Below in conjunction with accompanying drawing, the present invention is described further:

Fig. 1 is base station and position of mobile robot schematic diagram;

Fig. 2 infers a coordinate diagram for the mobile robot position of navigating;

Fig. 3 is ultrasonic laser positioning system schematic diagram; Fig. 3 (a) is a kind of preferred embodiment of ultrasonic laser emitter on base station; A kind of preferred embodiment that Fig. 3 (b) is the ultrasonic laser receiving trap on the mobile robot;

The circuit diagram that Fig. 4 is laser transmission circuit;

The circuit diagram that Fig. 5 is ultrasonic transmit circuit;

The circuit diagram that Fig. 6 is the second signal amplifying circuit;

The circuit diagram that Fig. 7 is phase discriminator;

The process flow diagram that Fig. 8 is information handling system;

The program flow diagram that Fig. 9 is the ultrasonic laser positioning system;

The experimental data figure that Figure 10 is the ultrasonic ranging accuracy;

Figure 11 is ultrasonic laser Locating System Accuracy experiments of measuring data plot;

Figure 12 is that the actual run trace of mobile robot and native system calculate the position coordinates obtained;

Figure 13 is the error schematic diagram between mobile robot's reentry point position and the theoretical position that calculates.

Embodiment

With reference to Fig. 1-9, the invention provides a kind of mobile robot positioning system, comprise base station 1;

Be installed on boat position on mobile robot 2 and infer positioning system, described boat position infers that positioning system comprises angular-rate sensor for obtaining mobile robot's 2 angle informations and for obtaining the displacement transducer of mobile robot's 2 travel distance information; Obtain the position coordinates of mobile robot 2 in positioning system is inferred in the boat position after merging angle information and travel distance information;

The ultrasonic laser positioning system, described ultrasonic laser positioning system comprises the ultrasonic laser emitter that is arranged on base station 1, is arranged on ultrasonic laser receiving trap and information handling system on mobile robot 2, by the message exchange between ultrasonic laser emitter and ultrasonic laser receiving trap, obtains the position coordinates of mobile robot 2 in the ultrasonic laser positioning system;

The data fusion unit, the fusion of positioning system angle information and travel distance information and the fusion that two position coordinateses in positioning system and ultrasonic laser positioning system are inferred in the boat position are inferred for the position of navigating in described data fusion unit; Realize the elimination of ultrasonic laser positioning system to cumulative errors in boat position supposition positioning system by the data fusion unit.

Angular-rate sensor wherein adopts gyroscope, by the integration to gyro data, can obtain the angle that the mobile robot turns over respect to initial position, but consider cost factor, has adopted MEMS digital gyro instrument cheaply; The rotating speed of this gyroscope energy highest measurement ± 320 °/sec, the angle random drift value is 0.016 °/sec.Digital interface is standard SPI serial output, and the SPI interface by microprocessor (DSP) can be configured and reading out data it.The program loop cycle is 20ms, in program, in each cycle period, by following formula, the data from MEMS digital gyro instrument is carried out to numerical integration:

θ _g(k)＝θ _g(k-1)+0.07326×20ms×(gyroData-gyroBias)；

Wherein gyroData is the data that read from MEMS digital gyro instrument by the SPI interface; GyroBias means the zero shift amount of MEMS digital gyro instrument, the gyro data read when the mobile robot is static; 20ms is integration period, i.e. the cycle period of program; 0.07326 be the conversion factor that gyro data is converted to angular velocity (spending/second) provided in MEMS digital gyro instrument technical documentation; θ _g(k-1) mean the MEMS digital gyro instrument angle value of a cycle period, θ _g(k) be by the resulting current MEMS digital gyro instrument angle of integration.

Displacement transducer is the incremental encoding code-disc, and the effect of incremental encoding code-disc is the distance of passing by for measuring the mobile robot.In mobile robot with two-wheel drive, all want the installation position displacement sensor on left and right two road wheels, each cycle period of positioning system Program reads the umber of pulse increment of incremental encoding code-disc on left and right two road wheels from robot base plate electric machine control module by the RS232 serial ports, the diameter of taking turns in conjunction with robot ambulation just can convert pulse increment to the distance increment that the mobile robot walks.

Δs＝(Δp _lc _l+Δp _rc _r)/2；

Wherein Δ s means the distance increment that in the program loop cycle, (20ms) mobile robot walks; Δ p _lwith Δ p _rmean that respectively incremental encoding code-disc on left and right two road wheels is at the pulse increment of a program loop in the cycle.Coefficient c _land c _rrespectively by Δ p _lwith Δ p _rconvert the distance that left and right road wheel is passed by one-period to.In advance by manual operation machine people along straight line travel distance l, the umber of pulse that reads left and right two incremental encoding code-discs is p _land p _r, can obtain c _land c _rbe respectively c _l=l/p _land c _r=l/p _r.

As shown in Figure 3, the ultrasonic laser emitter has comprised the drive unit that is arranged on ultrasonic transmitter 4, driving laser transmitter 3 and ultrasonic transmitter 4 synchronous rotaries on generating laser 3 with generating laser 3, the correspondence of photoelectric code disk 31; The ultrasonic laser receiving trap comprise laser pick-off array 5 that the laser pickoff by some annular spread forms, by some ultrasonic receivers form and with the ultrasound wave receiving array 6 of the corresponding installation of laser pick-off array 5.Wherein generating laser 3 adopts the linear laser transmitter, that is: the laser emitted is perpendicular to a laser rays on ground; The effect of code-disc is the emission angle of measuring laser.Eight laser pickoffs have been comprised in laser pick-off array 5 shown in Fig. 3, eight ultrasonic receivers have been comprised in ultrasound wave receiving array 6, each laser pickoff and ultrasonic receiver all a corresponding receiving side signal to, its objective is allow as far as possible the mobile robot multi-faceted receive laser and the ultrasound wave from base station 1.

The concrete structure of ultrasonic laser emitter and ultrasonic laser receiving trap can be with reference to Fig. 3 (a), 3 (b), drive unit has adopted motor 7, and realize transmission by a pair of gear set 8, those skilled in that art adopt other modes to be driven on the basis of realizing the native system function, but its purpose still will realize laser, hyperacoustic scanning, therefore also within covering scope of the present invention.

Positional information due to the mobile robot in environment, it is also important state towards angle, gyroscope measured towards there being random drift, therefore, a generating laser with photoelectric code disk 31 3 of same installation on the mobile robot is installed a laser pick-off array 5 on base station 1, is used for measuring base station 1 residing angle in mobile robot's coordinate system, in conjunction with mobile robot's coordinate, just can calculate the mobile robot towards angle.

With reference to also comprising the laser transmission circuit for regulating the Laser emission frequency, ultrasonic transmit circuit, second signal amplifying circuit and the phase discriminator for promoting the ultrasound wave emissive power in Fig. 4-7 ultrasonic laser positioning system.

With reference to Fig. 4, scan laser and the scan laser on base station on the mobile robot have different transmission frequencies, mutually interference-free, and the array of laser pick-off simultaneously also receives only the laser of characteristic frequency, is not subject to the interference of sunlight.By the resistance of regulating resistance R109, can make the square wave of phase-locked loop chip LM567 at TR pin output certain frequency, the transmission frequency that can regulate laser.The transmission frequency of the scan laser on base station 1 is adjusted to 20KHz, and the corresponding value of R109 is 4.7K; And the scan laser in robot is adjusted to 40KHz, the corresponding value of R109 is 2.35KHz.

With reference to Fig. 5, utilize chip LM567 to produce the required standard 40KHz square wave of hyperacoustic transmitter, because the required distance of ultrasonic ranging reaches more than 20 meters, so need to improve emissive power; Adopt motor drive ic L298P to drive ultrasonic transmitter, the control signal that EN_Ultrasonic is chip L298P, control hyperacoustic emission and close by low and high level for this reason; The 5V that IN1 is phaselocked loop LM567 generation, the square wave of 40KHz; OUT1, OUT2 are after chip LM298P amplifies and mutual reverse driving signal, are connected to two pins of ultrasonic transmitter, and another road of chip LM298P chip drives signal OUT3, OUT4 to be used for drive motor.

With reference to Fig. 6,7, for guaranteeing laser and hyperacoustic receiving range, the signal received in laser and ultrasound wave receiving array all must be processed through amplifying; Adopt operational amplifier LM324 to form a second amplifying circuit for this reason, signal carries out phase demodulation at the IN end that is sent to phaselocked loop LM567 after amplifying and excluding direct current signal, if frequency matching, it is at OUT output low level signal, and no person is high level signal; When the signal of OUT pin is low level from the high level saltus step, will excite the outside negative edge of F2808 to interrupt.

Secondly, the invention allows for a kind of method for positioning mobile robot, comprise the steps:

1) angular-rate sensor in boat position supposition positioning system and displacement transducer obtain respectively the mobile robot angle information and travel distance information; And angle information and travel distance information exchange are crossed to the fusion of data fusion unit, obtain the position coordinates that mobile robot in positioning system is inferred in the boat position;

2) the ultrasonic laser emitter on base station and the ultrasonic laser receiving trap on the mobile robot complete message exchange by information handling system, obtain the position coordinates of mobile robot in the ultrasonic laser positioning system;

3) in the position coordinates that mobile robot in positioning system is inferred in boat position and ultrasonic laser positioning system, mobile robot's position coordinates merges by the data fusion unit, eliminates the cumulative errors in boat position supposition positioning system.

Further explain the characteristics of positioning system of the present invention and localization method below in conjunction with positioning principle:

One, positioning system is inferred in the boat position

With reference to Fig. 1,2, after by MEMS digital gyro instrument and incremental encoding code-disc, obtaining mobile robot's angle information and travel distance information, by merging, just can obtain mobile robot's current location information.The position of setting the mobile robot means under world coordinate system, and base station 1 is fixed on the ground, the initial point that the position of setting 1 place, base station is world coordinate system, and base station 1 positive dirction is made as the y axle positive dirction of world coordinate system; The axis centre point of mobile robot's two road wheels represents the position at mobile robot place, and mobile robot's cephalad direction represents mobile robot's positive dirction.So, the position coordinates of robot and deflection form the state vector (x, y, θ) of robot.

With reference to Fig. 3, in order to obtain the current state of mobile robot, adopt the infinitesimal accumulate mode, mobile robot's action curve is regarded as to a lot of sections small rectilinear(-al)s, utilize like this travel distance information of gyrostatic angle information and code-disc, just can extrapolate the current positional information of robot from given initial position.The mobile robot goes to the changes in coordinates of A ' (x (k), y (k), θ (k)) from an A (x (k-1), y (k-1), θ (k-1)).Δ x, Δ y, Δ θ is illustrated respectively in the recruitment of a program loop cycle Δ t (20ms) time inner machine people's horizontal stroke, ordinate and angle, so Δ x, Δ y can be calculated by following formula:

$\mathrm{\Δx}={\mathrm{AA}}^{\′}\·\cos (\mathrm{\θ}(k-1)+\frac{\mathrm{\Δ\θ}}{2});$ $\mathrm{\Δy}={\mathrm{AA}}^{\′}\·\sin (\mathrm{\θ}(k-1)+\frac{\mathrm{\Δ\θ}}{2});$

Due to the time interval, Δ t is very short, and robot can be approximated to be straight line AA ' from an A point to A ' some path Δ s passed by:

$x\left(k\right)=x(k-1)+\mathrm{\Δx}$

$\≈x(k-1)+\mathrm{\Δs}\·\cos (\mathrm{\θ}(k-1)+\frac{\mathrm{\Δ\θ}}{2})$

$=x(k-1)+\mathrm{\Δs}\·\cos \left(\frac{\mathrm{\θ}(k-1)+(\mathrm{\θ}(k-1)+\mathrm{\Δ\θ})}{2}\right);$

$=x(k-1)+\mathrm{\Δs}\·\cos \left(\frac{\mathrm{\θ}(k-1)+\mathrm{\θ}\left(k\right)}{2}\right)$

$y\left(k\right)=y(k-1)+\mathrm{\Δy}$

$\≈y(k-1)+\mathrm{\Δs}\·\sin (\mathrm{\θ}(k-1)+\frac{\mathrm{\Δ\θ}}{2})$

$=y(k-1)+\mathrm{\Δs}\·\sin \left(\frac{\mathrm{\θ}(k-1)+(\mathrm{\θ}(k-1)+\mathrm{\Δ\θ})}{2}\right);$

$=y(k-1)+\mathrm{\Δs}\·\sin \left(\frac{\mathrm{\θ}(k-1)+\mathrm{\θ}\left(k\right)}{2}\right)$

Like this, from initial coordinate (x (0), y (0)) start, each program loop cycle (20ms) is at mobile robot's coordinate (x (k-1) of upper one-period, y (k-1)), on basis, calculate and once upgrade coordinate (x (k), y (k)), so circulate, just can constantly know the position coordinates at the current place of mobile robot.

Two, ultrasonic laser positioning system

With reference to Fig. 3,8,9, at first be to calculate the coordinate (x of mobile robot in base station coordinates system, y), with reference to Fig. 8, constantly flyback retrace of generating laser 3 on base station, after the scan laser that the laser pick-off array received on the mobile robot is sent to base station, central processing module produces external interrupt signal XINT1, and launch a marking signal (0x88) informing base station to base station by wireless module nRF905, illustrate that robot is scanned by it.After base station receives this marking signal, the angle [alpha] of the photoelectric code disk that will be connected with generating laser sends to the mobile robot by wireless module, and excites one section ultrasonic pulse.When the wireless module on the mobile robot receives data, excite the external interrupt XINT13 of central processing module, in this interrupt service routine writing time t ₁.Ultrasonic receiver module receives after ultrasound wave the external interrupt XINT2 that can excite central processing module, and writing time t ₂.Be transmitted as the light velocity in air due to electromagnetic wave, its transmission time can ignore, t time delay simultaneously launched from being wirelessly transmitted to ultrasound wave in base station ₀fix, so the aerial transmission time of ultrasound wave is t _f=t ₂-t ₁-t ₀.The aerial transmission speed of ultrasound wave is 340m/s, so the mobile robot between base station apart from d _rbjust can calculate, mobile robot's coordinate is:

x＝d _rbcos(α)；y＝d _rbsin(α)；

Set aforementioned boat position and infer that the position of mobile robot that positioning system is calculated constantly at k is denoted as (x _d(k), y _d(k) coordinates table that), the ultrasonic laser positioning system is calculated is shown (x _a, y _a), the position data of two systems is merged by following formula:

x(k)＝x _d(k)+k _x(x _a-x _d(k))；y(k)＝y _d(k)+k _y(y _a-y _d(k))；

K wherein _x, k _ymean Error Gain, value, at 0.5-1.0, is preferably 0.6; Then make (x _d(k), y _d(k))=(x (k), y (k)), eliminate the boat position and infer the cumulative errors in positioning system.

With reference to Fig. 3, the generating laser on the mobile robot sends scan laser, when scanning Shi， base station, base station, by wireless module, informs the mobile robot; Now the mobile robot reads the angle beta (being recorded by photoelectric code disk 31) of scan laser, and passes through the angle [alpha] of following formula calculating robot in base station coordinates system:

α＝arctan(y(k)/x(k))；

Wherein (x (k), y (k)) is the current mobile robot's who calculates position coordinates, the mobile robot who records by laser positioning so towards angle θ _lfor:

θ _l＝π-(β-α)；

Set k and constantly by the boat position, infer that the mobile robot that in positioning system, MEMS digital gyro instrument records is θ towards angle _g(k), by itself and θ _lmerge and just can obtain the mobile robot towards optimal estimation:

θ＝θ _g(k)+k _θ(θ _l-θ _g(k))；

Error Gain k wherein _θ=0.8, then make θ _d(k)=θ, thus the cumulative errors in the measurement of angle of MEMS digital gyro instrument eliminated.In data fusion process, Error Gain is selected larger, the data of ultrasonic laser positioning system are just larger to the corrective action of the data of boat position supposition positioning system, but itself there is error in the ultrasonic laser positioning system, so choose suitable Error Gain according to data statistics separately, with obtain position and towards optimal estimation.

Be understood that from foregoing the ultrasonic laser positioning system realizes by base station and the mutual exchange message of mobile robot, with reference to Fig. 8, laser pick-off array on base station is when receiving the scan laser of self-movement robot, send a confirmation signal by wireless module, mean that the mobile robot has scanned base station.External interrupt INT0 is used for the pulse to code-disc and is counted, and motor commutation is controlled.Pulse meeting of the every generation of code-disc causes interrupt INT 0, and program first judges the direction of laser scanning, then umber of pulse is added to 1 or subtract 1.When umber of pulse is greater than higher limit or is less than lower limit, control motor commutation, thereby realize the flyback retrace that comes of laser.The Z phase correction signal that external interrupt INT1 is code-disc, code-disc revolves the Z that turns around and only has mutually a pulse, and this signal is used for proofreading and correct the error produced in the code-disc step-by-step counting.External interrupt INT2 is connected to the matching addresses AM pin of wireless module, when central stopping pregnancy is given birth to, the wireless module that base station is described has received data, if data bit 0x88, illustrate that the mobile robot is arrived by base station scans, now base station sends to the mobile robot by the code-disc umber of pulse, and the excitation ultrasound wave impulse.

The present invention surveys mutually by sensors such as ultrasound wave, laser, then by statistics, filtering scheduling algorithm, carries out information fusion, obtains thus mobile robot's absolute position.This system is as a brand-new key control unit, can not only accurately implement location, accelerate robot response to external world, also avoided intricate reasoning, real-time and the applicability of positioning system significantly improve, generally be applicable to the exploitation of various intelligent mobile robots, as pipeline cleaning robot, grass-removing robot, skyscraper plaster robot, underground pipeline robot for overhauling, ground polishing machine device people, glass-cleaning robot, Astrobotic etc.

Below by several groups of accuracy and the precision of testing to verify positioning system of the present invention:

With reference to Figure 10, the experiment of first step ultrasonic ranging accuracy.It is the key of laser-ultrasound positioning system accurate positioning.The mobile robot is positioned in apart from base station different far and near 4 position: 240CM, 1200CM, 2100CM and 3000CM.In experiment when apart from base station, being greater than 35 meters, to such an extent as to the ultrasound wave receiving array on the too weak mobile robot of ultrasonic signal can't receive.Each range points is all tested 40 times.Record each some average error separately in experiment respectively: 2.9CM, 3.0CM, 5.4CM and 8.3CM.When the distance base station is less than or equal to 10 meters, absolute error is stabilized in 10CM usually.When robot will become greatly gradually apart from base station time error far away, when 20 meters left and right, distance base station, error remains on 10CM substantially.In the scope of 30 meters, measuring error is to accept at 10CM.The absolute error that obtains being greater than 15CM while accidentally measuring will application card Kalman Filtering policy filtering.

With reference to Figure 11, second step is the precision of assessment laser-ultrasound positioning system.Stop on a series of specified points of mobile robot in working environment, then according to the data message of laser positioning sensor and ultrasonic distance-measuring sensor, calculate the now position of robot.Because there is angular error in the laser positioning sensor of base station, causes the result of experimental point to be two groups and concentrate the trend that distribute.One has the scrambler of 1000 pulse resolutions and laser positioning sensor to link together, and in set of pulses signal, a pulse makes mistakes and will cause the measurement of angle of 0.36 degree error to occur.Hence one can see that, and high-resolution scrambler will contribute to improve positioning precision.Along with the distance between robot and base station becomes large, it is large that the site error that angle causes also can become.In addition, the error of every group of point is also relevant with ultrasonic measuring distance technology.As can be seen from Figure 11 the laser-ultrasound positioning system can effectively be worked in the scope of 30 meters of distance base stations.

With reference to Figure 12, this has organized experimental evaluation, and boat position supposition positioning system and ultrasonic laser positioning system merge rear mobile robot's performance.The error of Kalman filtering is at k _x=k _y=0.6 and k _θcan't calculate acquisition at=0.8 o'clock.In outdoor lawn, at first the manual drive mobile robot is along the path walking of 24 meters * 24 meters square white size tape labels on lawn one time, and the white size tape label only leads the mobile robot travelling on ideal path accurately with helping operating personnel.In this process, robot often travels that 60CM can calculate the position at place and by the data autostore, then sends to computing machine by serial port.Can find that from Figure 12 the position that calculates just in time roughly conforms to the run trace of robot reality, error is less.

With reference to Figure 13, allow the mobile robot along the path self-navigation itself of pre-programmed of square of 24 meters * 24 meters, mobile robot's software program keeps operation at reentry point always, and robot is after measuring end, continue to keep self-navigation, the ability of location continuously in the time of like this can Validity Test mobile robot long-play.Experiment is carried out 20 times, and each 2.5 minutes consuming time, clockwise with counterclockwise all tests.Although as shown in Figure 12, the distance of mobile robot and base station constantly becomes the increase that conference causes error, and when robot, during close to base station location, the accuracy of laser-ultrasound positioning system is also higher, so the reentry point site error is very little.Experimental results show that the ultrasonic laser positioning system can effective compensation the boat position infer the cumulative errors in positioning system.The mobile robot can work long hours continuously in the scope far away of 30 meters of distance base stations.And the border, lawn can be stored in robot, on the one hand avoid crossing the border, can allow on the other hand any position initialization of robot in the system effective range itself.

Experimental data, formula related in above-described embodiment are only the clearer positioning principle of the present invention of explaining of energy, practical range of the present invention is not construed as limiting, by above-mentioned technology contents, those skilled in the art can carry out multiple modification and conversion fully under the prerequisite that does not break away from the present invention spirit scope, but only otherwise the modification and the conversion that break away from spirit of the present invention all should belong to the defined scope of claims of the present invention.

Claims (11)

1. a mobile robot positioning system, is characterized in that: comprise base station (1);

Be installed on boat position on mobile robot (2) and infer positioning system, described boat position infers that positioning system comprises angular-rate sensor for obtaining mobile robot (2) angle information and for obtaining the displacement transducer of mobile robot (2) travel distance information; Obtain the position coordinates of mobile robot (2) in positioning system is inferred in the boat position after merging angle information and travel distance information;

The ultrasonic laser positioning system, described ultrasonic laser positioning system comprises the ultrasonic laser emitter be arranged on base station (1), be arranged on ultrasonic laser receiving trap and information handling system on mobile robot (2), obtain the position coordinates of mobile robot (2) in the ultrasonic laser positioning system by the message exchange between ultrasonic laser emitter and ultrasonic laser receiving trap, described ultrasonic laser emitter comprises the generating laser (3) with photoelectric code disk (31), correspondence is arranged on the ultrasonic transmitter (4) on generating laser (3), the drive unit of driving laser transmitter (3) and ultrasonic transmitter (4) synchronous rotary, the ultrasonic laser receiving trap comprise laser pick-off array (5) that the laser pickoff by some annular spread forms, by some ultrasonic receivers form and with the ultrasound wave receiving array (6) of the corresponding installation of laser pick-off array (5),

The data fusion unit, the fusion of positioning system angle information and travel distance information and the fusion that two position coordinateses in positioning system and ultrasonic laser positioning system are inferred in the boat position are inferred for the position of navigating in described data fusion unit; Realize the elimination of ultrasonic laser positioning system to cumulative errors in boat position supposition positioning system by the data fusion unit.

2. a kind of mobile robot positioning system according to claim 1, it is characterized in that: described angular-rate sensor is MEMS digital gyro instrument, and described displacement transducer is the incremental encoding code-disc.

3. a kind of mobile robot positioning system according to claim 1, it is characterized in that: have eight laser pickoffs in laser pick-off array (5) at least, have eight ultrasonic receivers in ultrasound wave receiving array (6) at least, each laser pickoff receiving side signal corresponding to ultrasonic receiver to.

4. a kind of mobile robot positioning system according to claim 1 and 2 is characterized in that: described information handling system comprises by laser, ultrasonic signal and triggers the central processing module of external interrupt and for the wireless module of exchange message.

5. a kind of mobile robot positioning system according to claim 1, is characterized in that: the generating laser (3) with photoelectric code disk (31) is installed on described mobile robot (2), laser pick-off array (5) is installed on base station (1).

6. a kind of mobile robot positioning system according to claim 1 and 2, it is characterized in that: described ultrasonic laser positioning system also comprises laser transmission circuit, the ultrasonic transmit circuit for promoting the ultrasound wave emissive power, second signal amplifying circuit and the phase discriminator for regulating the Laser emission frequency.

7. a kind of mobile robot positioning system according to claim 1 and 2, it is characterized in that: described data fusion unit is Kalman filter, by the Kalman filtering strategy, the position data of ultrasonic laser positioning system and boat position is inferred to the position data that positioning system obtains is merged.

8. a method for positioning mobile robot, is characterized in that comprising the steps:

1) angular-rate sensor in boat position supposition positioning system and displacement transducer obtain respectively mobile robot (2) angle information and travel distance information; And angle information and travel distance information exchange are crossed to the fusion of data fusion unit, obtain the position coordinates that mobile robot in positioning system is inferred in the boat position;

2) the ultrasonic laser emitter on base station (1) and the ultrasonic laser receiving trap on the mobile robot complete message exchange by information handling system, obtain the position coordinates of mobile robot (2) in the ultrasonic laser positioning system, described ultrasonic laser emitter comprises the drive unit that is arranged on ultrasonic transmitter (4), driving laser transmitter (3) and ultrasonic transmitter (4) synchronous rotary on generating laser (3) with generating laser (3), the correspondence of photoelectric code disk (31); The ultrasonic laser receiving trap comprise laser pick-off array (5) that the laser pickoff by some annular spread forms, by some ultrasonic receivers form and with the ultrasound wave receiving array (6) of the corresponding installation of laser pick-off array (5);

3) in the position coordinates that mobile robot (2) in positioning system is inferred in boat position and ultrasonic laser positioning system, mobile robot's (2) position coordinates merges by the data fusion unit, eliminates the cumulative errors in boat position supposition positioning system.

9. a kind of method for positioning mobile robot according to claim 8, is characterized in that: also comprise the correction of mobile robot towards angle, the steps include: that the generating laser (3) on mobile robot (2) sends scan laser scanning base station (1); After receiving scan laser, laser pick-off array (5) on base station (1) informs mobile robot (2) by wireless module; By the photoelectric code disk (31) on mobile robot (2) obtain scan laser angle and and then be converted into the angle information of mobile robot (2) in the coordinate system of base station (1); This angle information and boat position are inferred to the angle information that in positioning system, angular-rate sensor obtains is merged, eliminate the cumulative errors of angular-rate sensor.

10. a kind of method for positioning mobile robot according to claim 8, it is characterized in that: the message exchange step of described information handling system is:

1) generating laser (3) on base station (1) sends scan laser, trigger central processing module when the laser pick-off array (5) on mobile robot (2) receives this scan laser and produce external interrupt XINT1, and (1) sends marking signal to base station by wireless module;

2), after base station (1) receives marking signal, the angle information that will be arranged on the upper photoelectric code disk (31) in base station (1) is sent to mobile robot (2) by wireless module;

3) mobile robot (2) triggers central processing module generation external interrupt XINT13 after receiving angle information, and records time of reception;

4) ultrasonic transmitter (4) on base station (1) sends ultrasound wave, and the ultrasound wave receiving array (6) on mobile robot (2) triggers central processing module generation external interrupt XINT2 after receiving ultrasound wave, and records time of reception.

11. a kind of method for positioning mobile robot according to claim 8 is characterized in that: the boat position infers that the position coordinates of mobile robot in positioning system obtains by following formula:

$\begin{array}{c}x\left(k\right)=x(k-1)+\mathrm{\Δx}\\ \≈x(k-1)+\mathrm{\Δs}\·\cos (\mathrm{\θ}(k-1)+\frac{\mathrm{\Δ\θ}}{2})\\ =x(k-1)+\mathrm{\Δs}\·\cos \left(\frac{\mathrm{\θ}(k-1)+(\mathrm{\θ}(k-1)+\mathrm{\Δ\θ})}{2}\right)\\ =x(k-1)+\mathrm{\Δs}\·\cos \left(\frac{\mathrm{\θ}(k-1)+\mathrm{\θ}\left(k\right)}{2}\right)\end{array};$

$\begin{array}{c}y\left(k\right)=y(k-1)+\mathrm{\Δy}\\ \≈y(k-1)+\mathrm{\Δs}\·\sin (\mathrm{\θ}(k-1)+\frac{\mathrm{\Δ\θ}}{2})\\ =y(k-1)+\mathrm{\Δs}\·\sin \left(\frac{\mathrm{\θ}(k-1)+(\mathrm{\θ}(k-1)+\mathrm{\Δ\θ})}{2}\right);\\ =y(k-1)+\mathrm{\Δs}\·\sin \left(\frac{\mathrm{\θ}(k-1)+\mathrm{\θ}\left(k\right)}{2}\right)\end{array}$

Δ x wherein, Δ y, Δ θ mean respectively the mobile robot in a cycle period from (x (k-1), y (k-1), the recruitment of horizontal ordinate x, ordinate y and angle θ while θ (k-1)) putting to (x (k), y (k), θ (k)); Δ s means mobile robot's path that point is passed by from (x (k-1), y (k-1), θ (k-1)) point to (x (k), y (k), θ (k));

In the ultrasonic laser positioning system, mobile robot's position coordinates obtains by following formula:

X=d _rbcos (α); Y=d _rbsin (α); D wherein _rbmean that the mobile robot is to the distance between base station; α means on base station the angle-data that photoelectric code disk records;

Above-mentioned boat position is inferred to the position of mobile robot coordinate that positioning system is calculated constantly at k is made as (x _d(k), y _d(k) position coordinates that), the ultrasonic laser positioning system is calculated is made as (x _a, y _a), the position coordinates of two systems is merged by following formula:

x(k)＝x _d(k)+k _x(x _a-x _d(k))；y(k)＝y _d(k)+k _y(y _a-y _d(k))；

K wherein _x, k _ymean Error Gain, value is at 0.5-1.0; Then make (x _d(k), y _d(k))=(x (k), y (k)), eliminate the boat position and infer the cumulative errors in positioning system.

Images