CN102121827A - 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 intelligence 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 it is also higher to the requirement of robot, location technology wherein is a very critical technical problems, and the overall situation is located mobile robot's a critical function especially, do not have this function, any autokinetic movement of robot all is blindly.No matter be which kind of robot, it will solve three problems at the volley all the time, promptly " 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.

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

To sum up, present mobile robot is far from reaching the requirement of practicability, its intelligent various demands that also satisfy 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 in that localization for Mobile Robot is technical.

Summary of the invention

Problem to be solved by this invention just provides a kind of mobile robot positioning system and localization method thereof, can accurately implement the 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 the base station;

Positioning system is inferred in the boat position that is installed on the mobile robot, and described boat position infers that positioning system comprises the displacement transducer that is used to obtain the angular-rate sensor of mobile robot's angle information and is used to obtain mobile robot's travel distance information; Obtain the position coordinates of mobile robot in boat position supposition positioning system 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 installed on the base station, is installed in ultrasonic laser receiving trap and information handling system on the mobile robot, obtains the position coordinates of mobile robot in the ultrasonic laser positioning system by the message exchange between ultrasonic laser emitter and the ultrasonic laser receiving trap;

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

Further, described angular-rate sensor is a MEMS numeral gyroscope, and described displacement transducer is the incremental encoding code-disc.

Further, described ultrasonic laser emitter comprises that the generating laser, the correspondence that have photoelectric code disk are installed in the drive unit that ultrasonic transmitter, driving laser transmitter and ultrasonic transmitter on the generating laser rotate synchronously; The ultrasonic laser receiving trap comprise the laser pick-off array formed by the laser pickoff of some annular spread, 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, the corresponding receiving side signal of each laser pickoff with ultrasonic receiver to.Its objective is allow as far as possible the mobile robot multi-faceted receive laser and ultrasound wave from the base station.

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

Further, described mobile robot goes up the generating laser that has photoelectric code disk is installed, and the laser pick-off array is installed on the base station.The positional information of mobile robot in environment, it also is important state towards the angle.Gyroscope measured towards there being random drift, for this reason, the same generating laser that a tape code dish is installed on the mobile robot, a laser pick-off array is installed on the 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 the angle.

Further, described ultrasonic laser positioning system also comprises the laser transmission circuit that is used to regulate the Laser emission frequency, the ultrasonic transmit circuit that is used to promote ultrasonic emitting power, second signal amplifying circuit and phase discriminator.Adjusting by laser transmission circuit makes that scan laser has no transmission frequency on scan laser on the mobile robot and the base station, interference-free mutually, simultaneously also make the laser pick-off array receive only the laser of characteristic frequency, be not subjected to the interference of external environment light; By ultrasonic transmit circuit hyperacoustic measuring distance is reached more than 20 meters, increase mobile robot's sphere of action; And second signal amplifying circuit and phase discriminator are realized the processing and amplifying and the phase wave demodulation of received signal on the laser pickoff, guarantee laser and ultrasonic reception distance.

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

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) angle information and the travel distance information of obtaining the mobile robot respectively by angular-rate sensor in the boat position supposition positioning system and displacement transducer; And angle information and travel distance information merged by the data fusion unit, obtain the position coordinates that mobile robot in the positioning system is inferred in the boat position;

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

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

Further, also comprise the correction of mobile robot, the steps include: that the generating laser on the mobile robot sends scan laser scanning base station towards the angle; When the laser pick-off array received on the base station is informed the mobile robot by wireless module behind 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 that the angle information that angular-rate sensor obtains in the positioning system merges, and eliminates the cumulative errors of angular-rate sensor.

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

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

2) after the base station received marking signal, the angle information of photoelectric code disk was sent to the mobile robot by wireless module on the base station with being installed in;

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

4) ultrasonic transmitter on the 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 the recorder time.

Further, the boat position infers that the position coordinates of mobile robot in the 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, Δ θ represent that respectively the mobile robot puts the recruitment of (x (k), y (k), θ (k)) when point horizontal ordinate x, ordinate y and angle θ from (x (k-1), y (k-1), θ (k-1)) in a cycle period; Δ s represents that the mobile robot puts the path that (x (k), y (k), θ (k)) point is passed by from (x (k-1), y (k-1), θ (k-1));

Mobile robot's position coordinates obtains by following formula in the ultrasonic laser positioning system:

X=d _RbCos (α); Y=d _RbSin (α); D wherein _RbThe mobile robot is to the distance between the base station in expression; α represents the angle-data that photoelectric code disk records on the base station;

Above-mentioned boat position is inferred that positioning system is made as (x at mobile robot's position coordinates that k calculates constantly _d(k), y _d(k)), the position coordinates that calculates of ultrasonic laser positioning system is made as (x _a, y _a), then the position coordinates of two systems merges 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 _yThe expression Error Gain, value is at 0.5-1.0; Make (x then _d(k), y _d(k))=(x (k), y (k)), the cumulative errors in the positioning system is inferred in elimination boat position.

Beneficial effect of the present invention:

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

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

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

4, by the application of ultrasonic laser positioning system, significantly improve mobile robot's bearing accuracy, also prolonged mobile robot's working time simultaneously, helped improving mobile robot's operation performance;

5, the mobile robot can both 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 the location, accelerates robot response to external world, avoids intricate reasoning, and the real-time and the applicability of positioning system significantly improve.

Description of drawings

The present invention is described further below in conjunction with accompanying drawing:

Fig. 1 is base station and mobile robot's position view;

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

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

Fig. 4 is the circuit diagram of laser transmission circuit;

Fig. 5 is the circuit diagram of ultrasonic transmit circuit;

Fig. 6 is the circuit diagram of second signal amplifying circuit;

Fig. 7 is the circuit diagram of phase discriminator;

Fig. 8 is the process flow diagram of information handling system;

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

Figure 10 is the experimental data figure of ultrasonic ranging accuracy;

Figure 11 is a ultrasonic laser positioning system precision measure experimental data figure;

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

Figure 13 is the error synoptic 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;

Positioning system is inferred in the boat position that is installed on the mobile robot 2, and described boat position infers that positioning system comprises the displacement transducer that is used to obtain the angular-rate sensor of mobile robot's 2 angle informations and is used to obtain mobile robot's 2 travel distance information; Obtain the position coordinates of mobile robot 2 in boat position supposition positioning system 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 installed on the base station 1, is installed in ultrasonic laser receiving trap and information handling system on the mobile robot 2, obtains the position coordinates of mobile robot 2 in the ultrasonic laser positioning system by the message exchange between ultrasonic laser emitter and the ultrasonic laser receiving trap;

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

Angular-rate sensor wherein adopts gyroscope, by the integration to gyro data, can obtain the mobile robot with respect to the angle that initial position turned over, but consider cost factor, has adopted MEMS numeral gyroscope cheaply; This gyroscope can highest measurement ± 320 °/rotating speed of sec, and 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 of data it.The program loop cycle is 20ms, in the program in each cycle period by following formula to carrying out numerical integration from the gyrostatic data of MEMS numeral:

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

Wherein gyroData is the data that read from MEMS numeral gyroscope by the SPI interface; GyroBias represents MEMS numeral gyrostatic zero shift amount, the i.e. gyro data that is read when the mobile robot is static; 20ms is an integration period, i.e. the cycle period of program; 0.07326 be provide in the MEMS numeral gyroscope technical documentation gyro data is converted to the angular velocity conversion factor of (degree/second); θ _g(k-1) the MEMS numeral gyroscope angle value of the last cycle period of expression, θ _g(k) be by the resulting current MEMS numeral gyroscope angle of integration.

Displacement transducer is the incremental encoding code-disc, and the effect of incremental encoding code-disc is to be used to measure the distance that the mobile robot passes by.Mobile apparatus philtrum with two-wheel drive, all want the installation position displacement sensor on left and right two road wheels, each cycle period of program reads the umber of pulse increment of incremental encoding code-disc on left and right two road wheels by the RS232 serial ports in the positioning system from robot base plate electric machine control module, just pulse increment can be converted to the distance increment that the mobile robot walks in conjunction with the diameter of robot ambulation wheel.

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

Wherein Δ s represents (20ms) distance increment that the mobile robot walked in the program loop cycle; Δ p _lWith Δ p _rRepresent 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 with Δ p _lWith Δ p _rConvert the distance that left and right road wheel is passed by in one-period.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 that the generating laser 3, the correspondence that have photoelectric code disk 31 are installed in ultrasonic transmitter 4, the driving laser transmitter 3 and the ultrasonic transmitter 4 synchronous drive units that rotate on the generating laser 3; The ultrasonic laser receiving trap comprise the laser pick-off array 5 formed by the laser pickoff of some annular spread, by some ultrasonic receivers form and with the ultrasound wave receiving array 6 of laser pick-off array 5 corresponding installations.Wherein generating laser 3 adopts the linear laser transmitter, that is: the laser that emits is perpendicular to a laser rays on ground; The effect of code-disc is the emission angle of Laser Measurement.Eight laser pickoffs have been comprised in the laser pick-off array 5 shown in Fig. 3, eight ultrasonic receivers have been comprised in the 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 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 by a pair of gear set 8 realization transmissions, those skilled in that art adopt other modes to drive on the basis of realizing the native system function, but its purpose still will realize laser, hyperacoustic scanning, so also within covering scope of the present invention.

Because the positional information of mobile robot in environment, it also is important state towards the angle, gyroscope measured towards there being random drift, therefore, the same generating laser 3 that a band photoelectric code disk 31 is installed is installed a laser pick-off array 5 on base station 1 on the mobile robot, 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 the angle.

With reference to also comprising the laser transmission circuit that is used to regulate the Laser emission frequency, the ultrasonic transmit circuit that is used to promote ultrasonic emitting power, second signal amplifying circuit and phase discriminator in Fig. 4-7 ultrasonic laser positioning system.

With reference to Fig. 4, the scan laser on the mobile robot has different transmission frequencies with scan laser on the base station, and is interference-free mutually, and the array of laser pick-off simultaneously also receives only the laser of characteristic frequency, is not subjected to the interference of sunlight.By regulating the resistance of resistance R 109, can make the square wave of phase-locked loop chip LM567, i.e. the transmission frequency of scalable laser at TR pin output certain frequency.The transmission frequency of the scan laser on the base station 1 is adjusted to 20KHz, and the corresponding value of R109 is 4.7K; And the scan laser in the robot is adjusted to 40KHz, and 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, EN_Ultrasonic is the control signal of chip L298P for this reason, controls hyperacoustic emission and closes by high-low level; The 5V that IN1 produces for phaselocked loop LM567, the square wave of 40KHz; OUT1, OUT2 are connected to two pins of ultrasonic transmitter for amplify back and reverse drive signal mutually through chip LM298P, and another road drive signal OUT3, the OUT4 of chip LM298P chip is used for drive motor.

With reference to Fig. 6,7, for guaranteeing laser and ultrasonic reception distance, the signal that receives in laser and ultrasound wave receiving array all must be through processing and amplifying; Adopt operational amplifier LM324 to form a second amplifying circuit for this reason, signal carries out phase demodulation through amplification and after excluding direct current signal at the IN end that is sent to phaselocked loop LM567, if frequency matching, then it is at OUT output low level signal, and the person is not a high level signal; When the signal of OUT pin when the high level saltus step is low level, 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) angle information and the travel distance information of obtaining the mobile robot respectively by angular-rate sensor in the boat position supposition positioning system and displacement transducer; And angle information and travel distance information merged by the data fusion unit, obtain the position coordinates that mobile robot in the positioning system is inferred in the boat position;

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

3) mobile robot's position coordinates merges by the data fusion unit in the boat position position coordinates of inferring mobile robot in the positioning system and the ultrasonic laser positioning system, eliminates the cumulative errors in the 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 obtaining mobile robot's angle information and travel distance information, by merging the current location information that just can obtain the mobile robot by MEMS numeral gyroscope and incremental encoding code-disc.Set mobile robot's position and represent under world coordinate system, base station 1 is fixed on the ground, and the position of setting 1 place, base station is the initial point of 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 is represented the position at mobile robot place, and mobile robot's cephalad direction is represented mobile robot's positive dirction.So, and the state vector of the position coordinates of robot and deflection composition robot (x, y, θ).

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 a lot of sections small rectilinear(-al)s, utilize the travel distance information of gyrostatic angle information and code-disc like this, just can extrapolate the robot current position information 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, Δ θ are 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});$

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

$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)) beginning, each program loop cycle (20ms) is all at mobile robot's coordinate (x (k-1) of last one-period, y (k-1)) on the basis, calculates and once upgrade coordinate (x (k), y (k)), so circulate, just can know the position coordinates at the current place of mobile robot constantly.

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, generating laser 3 flyback retrace constantly on the base station, behind the scan laser that the laser pick-off array received on the mobile robot is sent to the base station, central processing module produces external interrupt signal XINT1, and pass through wireless module nRF905 to a marking signal of base station (0x88) informing base station, illustrate that robot is scanned by it.After the base station received this marking signal, the angle [alpha] of the photoelectric code disk that will be connected with generating laser sent to the mobile robot by wireless module, and excited 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 the external interrupt XINT2 that can excite central processing module behind the ultrasound wave, and writing time t ₂Because electromagnetic wave is transmitted as the light velocity in air, its transmission time can ignore, simultaneously in the base station from being wirelessly transmitted to t time delay of ultrasonic emitting ₀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 the 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 positioning system is (x at mobile robot's location mark that k calculates constantly _d(k), y _d(k)), the coordinate representation of calculating of ultrasonic laser positioning system is (x _a, y _a), then the position data of two systems merges 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 _yThe expression Error Gain, value is preferably 0.6 at 0.5-1.0; Make (x then _d(k), y _d(k))=(x (k), y (k)), the cumulative errors in the positioning system is inferred in elimination boat position.

With reference to Fig. 3, the generating laser on the mobile robot sends scan laser, and when scanning the base station, the mobile robot is informed by wireless module in the base station; This moment, the mobile robot read the angle beta (being recorded by photoelectric code disk 31) of scan laser, and passed 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, so the mobile robot's who records by laser positioning towards angle θ _lFor:

θ _l＝π-(β-α)；

Set k and infer that by the boat position mobile robot that MEMS numeral gyroscope records in the positioning system is θ towards the angle constantly _g(k), with 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, make θ then _d(k)=and θ, thus the cumulative errors in the measurement of angle of elimination MEMS numeral gyroscope.In data fusion process, Error Gain is selected big more, the data of ultrasonic laser positioning system are just big more 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 the 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 the base station is when receiving the scan laser of self-movement robot, send a confirmation signal by wireless module, the expression mobile robot has scanned the base station.External interrupt INT0 is used for to code-disc pulse and counts i.e. motor commutation control.Pulse meeting of the every generation of code-disc causes interrupt INT 0, and program is judged the direction of laser scanning earlier, umber of pulse is added 1 or subtract 1 again.When umber of pulse during, control motor commutation, thereby realize the flyback retrace that comes of laser greater than higher limit or less than lower limit.External interrupt INT1 is the Z phase correction signal of code-disc, and code-disc revolves the Z that turns around and has only a pulse mutually, and this signal is used for proofreading and correct the error that produces 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 the base station is described has received data, if data bit 0x88, illustrate that the mobile robot is arrived by base station scans, this moment, the base station sent to the mobile robot with the code-disc umber of pulse, and the excitation ultrasound wave impulse.

The present invention surveys mutually by sensors such as ultrasound wave, laser, carries out information fusion by statistics, filtering scheduling algorithm then, obtains mobile robot's absolute position thus.This system is as a brand-new key control unit, can not only accurately implement the location, accelerate robot response to external world, also avoided intricate reasoning, the 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 people, machine people, Astrobotic etc. clean the windows.

Test accuracy and the precision of verifying positioning system of the present invention below by several groups:

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 the experiment when apart from the base station during 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.Recording each some average error separately in the experiment is respectively: 2.9CM, 3.0CM, 5.4CM and 8.3CM.When distance base station during smaller or equal to 10 meters, absolute error is stabilized in the 10CM usually.When robot will become greatly gradually apart from base station time error far away, during 20 meters left and right sides, error remains on 10CM substantially in the distance base station.In 30 meters scope, measuring error is to accept at 10CM.Obtaining during accidental the measurement will application card Kalman Filtering policy filtering greater than the absolute error of 15CM.

With reference to Figure 11, second step was the precision of assessment laser-ultrasound positioning system.Stop on a series of specified points of mobile robot in working environment, calculate the position of robot this moment then according to the data message of laser positioning sensor and ultrasonic ranging sensor.Because there is angular error in the laser positioning sensor of base station, cause the result of experimental point to be two groups of trend of concentrating distribution.One has the scrambler of 1000 pulse resolutions and laser positioning sensor to link together, and a pulse makes mistakes and will cause the measurement of angle of 0.36 degree error to occur in set of pulses signal.Hence one can see that, and high-resolution scrambler will help to improve bearing accuracy.Along with the distance between robot and base station becomes big, it is big 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 can effectively work in the scope of 30 meters of laser-ultrasound positioning systems in the distance base station.

With reference to Figure 12, mobile robot's a performance after this has organized experimental evaluation boat position supposition positioning system and ultrasonic laser positioning system merge.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 path walking of manual drive mobile robot 24 meters * 24 meters square white size tape labels on the lawn is one time, and the white size tape label only leads the mobile robot travelling on the ideal path accurately with helping operating personnel.In this process, the 60CM that whenever travels of robot can calculate the position at place and data are stored automatically, sends to computing machine by serial port again.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 24 meters * 24 meters the square path self-navigation of pre-programmed itself, 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 all tests counterclockwise.Though as shown in Figure 12, the distance of mobile robot and base station constantly becomes the increase that conference causes error, and when robot approached base station location, the accuracy of laser-ultrasound positioning system was also higher, so the reentry point site error is very little.Experimental results show that the ultrasonic laser positioning system can effectively compensate the cumulative errors in the boat position supposition positioning system.The mobile robot can work long hours in the 30 meters scopes far away in distance base station continuously.And the border, lawn can be stored in the robot, on the one hand avoids crossing the border, and can allow any position initialization of robot in system's effective range itself on the other hand.

Experimental data, formula related in the foregoing description only are 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 (12)

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

Positioning system is inferred in the boat position that is installed on the mobile robot (2), and described boat position infers that positioning system comprises the displacement transducer that is used to obtain the angular-rate sensor of mobile robot (2) angle information and is used to obtain mobile robot (2) travel distance information; Obtain the position coordinates of mobile robot (2) in boat position supposition positioning system 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 installed on the base station (1), is installed in ultrasonic laser receiving trap and information handling system on the mobile robot (2), obtains the position coordinates of mobile robot (2) in the ultrasonic laser positioning system by the message exchange between ultrasonic laser emitter and the ultrasonic laser receiving trap;

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

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

3. a kind of mobile robot positioning system according to claim 2 is characterized in that: described ultrasonic laser emitter comprises that the generating laser (3), the correspondence that have photoelectric code disk (31) are installed in ultrasonic transmitter (4), the driving laser transmitter (3) and the synchronous drive unit that rotates of ultrasonic transmitter (4) on the generating laser (3); The ultrasonic laser receiving trap comprise the laser pick-off array (5) formed by the laser pickoff of some annular spread, by some ultrasonic receivers form and with the ultrasound wave receiving array (6) of the corresponding installation of laser pick-off array (5).

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

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

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

7. according to claim 1 or 2 or 3 described a kind of mobile robot positioning systems, it is characterized in that: described ultrasonic laser positioning system also comprises the laser transmission circuit that is used to regulate the Laser emission frequency, the ultrasonic transmit circuit that is used to promote ultrasonic emitting power, second signal amplifying circuit and phase discriminator.

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

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

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

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

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

10. a kind of method for positioning mobile robot according to claim 9 is characterized in that: also comprise the correction of mobile robot towards the angle, the steps include: that the generating laser (3) on the mobile robot (2) sends scan laser scanning base station (1); After receiving scan laser, the laser pick-off array (5) on base station (1) informs mobile robot (2) by wireless module; By the photoelectric code disk (31) on the 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 that the angle information that angular-rate sensor obtains in the positioning system merges, and eliminates the cumulative errors of angular-rate sensor.

11. a kind of method for positioning mobile robot according to claim 9 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, when the laser pick-off array (5) on the mobile robot (2) receives this scan laser, trigger central processing module and produce external interrupt XINT1, and send marking signal to base station (1) by wireless module;

2) after base station (1) received marking signal, the angle information that will be installed in the last photoelectric code disk (31) in base station (1) was 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 the recorder time;

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

12. a kind of method for positioning mobile robot according to claim 9 is characterized in that: the boat position infers that the position coordinates of mobile robot in the 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, Δ θ represent that respectively the mobile robot puts the recruitment of (x (k), y (k), θ (k)) when point horizontal ordinate x, ordinate y and angle θ from (x (k-1), y (k-1), θ (k-1)) in a cycle period; Δ s represents that the mobile robot puts the path that (x (k), y (k), θ (k)) point is passed by from (x (k-1), y (k-1), θ (k-1));

Mobile robot's position coordinates obtains by following formula in the ultrasonic laser positioning system:

X=d _RbCos (α); Y=d _RbSin (α); D wherein _RbThe mobile robot is to the distance between the base station in expression; α represents the angle-data that photoelectric code disk records on the base station;

Above-mentioned boat position is inferred that positioning system is made as (x at mobile robot's position coordinates that k calculates constantly _d(k), y _d(k)), the position coordinates that calculates of ultrasonic laser positioning system is made as (x _a, y _a), then the position coordinates of two systems merges 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 _yThe expression Error Gain, value is at 0.5-1.0; Make (x then _d(k), y _d(k))=(x (k), y (k)), the cumulative errors in the positioning system is inferred in elimination boat position.

Images