Utility model content
The purpose of this utility model be overcome prior art above-mentioned deficiency and provide a kind of positioning precision high, good reliability, the robot localization navigational system that dirigibility is high.
The technical solution of the utility model is:
A kind of robot localization navigational system, comprises the beacon module of multiple fixed position, at least one robot localization terminal and electronic navigation platform;
Described beacon module comprises:
First radio receiving transmitting module, for getting in touch with robot localization terminal and/or electronic navigation platform wirelessly, transmitting-receiving instruction and synchronizing signal;
Ultrasound wave transmitter module, for sending the ultrasonic signal addressed;
First intelligent processing module, for the treatment of the instruction received and storage ultrasound wave coding, the arrangement of process synchronous sequence;
Described robot localization terminal comprises:
Second radio receiving transmitting module, gets in touch with beacon module and electronic navigation platform for being responsible for, transmitting/receiving wireless synchronizing signal, and receives the scheduling made ultrasonic signal to be received;
Ultrasound wave receiving array, for receiving the ultrasonic signal from different angles;
Second intelligent processing module, comprises ARRAY PROCESSING module and central processing unit;
Described ARRAY PROCESSING module is used for the ultrasonic signal received to amplify, and the power according to its signal carries out gain control automatically, the ultrasound wave simulating signal intensity of output is kept relative stability, and converts the simulating signal received to digital signal;
Described central processing unit for the treatment of the instruction received, module each several part logical relation, make the ultrasound data information that synchronous sequence arrangement, recognition coding, storage and analysis receive, and ultrasound data is put into computing unit together together with the electronic chart that electronic navigation platform sends carry out computing, obtain position coordinate data;
Described electronic navigation platform is for storing the coding of whole robot, build electronic chart, planning is the moving target of calculating robot or the position of path and beacon module also, makes robot can both obtain the alternate covering of beacon module in the range of movement allowed; And electronic chart is wirelessly passed to the scheduling that robot localization terminal carries out robot; Synchronous also for coordinated beacon module and robot localization terminal, receives the position coordinate data uploaded from each robot localization terminal and shows.
Further, the top of described ultrasound wave receiving array is equipped with barrier scanner; Described barrier scanner comprises synchronous rotation system and control system;
Described synchronous rotation system comprises the support be located on ultrasound wave receiving array, servomotor installed by support, servomotor connects compound sensor seat by rotating boarded head, compound sensor seat is provided with human body sensor and ultrasonic transmission/reception sensor, the lower end of servomotor connects scrambler;
Described control system comprises motor drive module and barrier scanning main control module, the output terminal of described barrier scanning main control module connects motor drive module, the input end of barrier scanning main control module connects scrambler, human body sensor and ultrasonic transmission/reception sensor respectively, and the output terminal of motor drive module connects servomotor; Described barrier scanning main control module connects electronic navigation platform and robot localization terminal respectively by CAN.
Further, described ultrasonic transmission/reception sensor scans main control module by ultrasonic signal amplifier with described barrier and is connected, and the ultrasonic signal that described ultrasonic signal amplifier is used for the reception of ultrasonic transmission/reception sensor and transmission amplifies.
Further, the front end of described ultrasonic transmission/reception sensor is provided with the sound-absorbing pipe for narrowing ultrasound wave receiving angle.
Further, described ARRAY PROCESSING module comprises pre-amplifying module and A/D modular converter;
Described pre-amplifying module is used for the ultrasonic signal received to amplify, and the power according to its signal carries out gain control automatically, and the ultrasound wave simulating signal intensity of output is kept relative stability;
Described A/D modular converter is used for the ultrasound wave simulating signal in detection time section convert digital signal to continuously and store; The ultrasonic signal amplitude arrived by central processing unit scanning collection, processes this group ultrasound data; Super according to what receive
The average amplitude of sound wave pulse group, calculates the threshold value for judging; The point reaching judgment threshold then thinks that the ultrasonic signal of current test arrives starting point, otherwise thinks noise signal.
Further, described electronic navigation platform comprises:
Carrying the computing machine of navigation software, for storing the coding of whole robot, building electronic chart, as required dispatch robot, control multiple stage robot and complete respective work simultaneously;
3rd radio receiving transmitting module, for transmitting electronic chart to robot localization terminal, and send positioning synchronous signal, the sequential of coordinated beacon module and robot localization terminal, receive position coordinate data that each robot localization terminal uploads and the announcement information that barrier scanner is uploaded;
Display, for showing the navigation information on electronic chart;
Electronic chart computing module, for planning and the moving target of calculating robot or the position of path and beacon module, generates navigation information.
Further, described electronic chart comprises the electronic plane map corresponding with robot motion region and beacon module overlay area difference and electronic three-dimensional map.
Further, described ultrasonic transmission/reception array is the integrated array with at least four ultrasonic sensing units, or by least four independently ultrasonic sensor disperse array; Described pre-amplifying module is corresponding with the quantity of ultrasonic sensing unit with the number of channels of A/D modular converter.
The utility model compared with prior art has following features:
(1) ultrasonic signal received is carried out automatic growth control according to the power of its signal, the dynamic range of stable output, and convert digital signal to, calculate judgment threshold, judge that current demand signal is ultrasonic signal or noise signal according to judgment threshold, thus realize ultrasound examination, solve the attenuation problem of ultrasonic signal in transmitting procedure;
(2) phase differential in ultrasonic array between ultrasonic sensing unit and signal intensity is utilized, differentiate the ultrasound wave of different approaches, the ultrasound wave that cut-off line arrives, like this can solving multipath effect and non-line-of-sight propagation problem, the interference of filtering environmental sound wave, shorten the test duration, greatly improve distance accuracy and reliability;
(3) whether the ultrasound wave using coding ultrasound wave mode can be convenient to determine to receive is that the beacon module needed sends, simultaneously by the theoretical code comparison with emissive source, judge that bundle ultrasound wave that the beacon module whether this group ultrasound wave needs sends, can accurately determine hyperacoustic starting point and emission angle, improve the accuracy of range finding;
(4) ultrasound wave receiving array received ultrasonic signal is utilized, through data processing, position and the direction of robot is judged according to the phase relation of the ultrasonic signal between ultrasound wave receiving array and beacon module, and be sent to electronic location platform together with electronic chart, realize path planning and the location of robot;
(5) ultrasonic signal of more than 2 beacon module gathered is utilized, to calculate between absolute distance between beacon module and relative distance percentage error as compensation, and then calculate the accurate velocity of sound under current environmental condition, accurate rate is high, cost is low, calculates simple;
(6) by scanning and data processing, barrier can be calculated and whether exist and the size of barrier.Robot can choice for use barrier treatment mechanism better, both can select to be stop detouring when also waiting for according to barrier size, again can for the difference of barrier size, make corresponding disposal, when the barrier run into is larger, when will have an impact to measurement result, other beacon can be selected in time to position robot, there will not be the positioning error because ultrasonic wave path problem on obstacle produces, when the barrier run into is less, can not to when having influence on measuring accuracy, the beacon module in former positioning planning can be used to position, make robot positioning system more intelligent,
(7) by human body sensor to the induction of barrier and follow-up data processing, judge barrier whether as people.When barrier is behaved, supply information to robot, robot is adopted and stays where one is, pending further orders or greet and please its mode left evade; When barrier is inhuman, supply information to robot, make robot adopt conventional workaround to evade this barrier; Robot can obtain the information that barrier is people or thing, carries out the foundation of being correlated with when disposing, and realize the interaction of robot and the mankind to provide robot;
(8) when robot departs from default movement locus, still can according in other movement locus be located near robot the beacon module planned realize location automatically, thus avoid robot bias motion track just lost, the problem of cisco unity malfunction.
Embodiment
Below in conjunction with the drawings and specific embodiments, detailed construction of the present utility model is further described.
A kind of robot localization navigational system, comprises multiple beacon module, at least one robot localization terminal, barrier scanner and electronic navigation platform.Robot localization terminal is arranged in robot, and robot can carry the robot localization terminal that quantity does not wait as required.The position of beacon module must be fixing, thus obtains fixed position coordinate, and it can be installed on (position of placement does not have specific requirement) on wall or on other fixed object, and concrete quantity determines according to the space size of position undetermined.The movement locus of robot arranges multiple position coordinates, calculates at least two beacon module cooked up needed for these position coordinates places difference.
Particularly, beacon module comprises the first intelligent processing module and the first radio receiving transmitting module be connected with the first intelligent processing module respectively and ultrasound wave transmitter module.
First radio receiving transmitting module is used for getting in touch with the robot localization terminal needing to locate and/or electronic navigation platform wirelessly, transmitting-receiving instruction and synchronizing signal; First radio receiving transmitting module can adopt the mixed-media network modules mixed-media meeting various wireless communication agreement, such as WIFI module, ZigBee module, bluetooth communication etc.Preferably, the first radio receiving transmitting module adopts wireless serial communication module.
Ultrasound wave transmitter module, for sending the ultrasonic signal addressed; Because beacon module each in multiple beacon module gives unique encodings, so that by recognition coding, robot localization terminal judges whether the ultrasound wave received is that called beacon module sends.
First intelligent processing module is for the treatment of the instruction received and store ultrasound wave coding, and processes synchronous sequence arrangement.
Particularly, robot localization terminal comprises the second intelligent processing module and the second radio receiving transmitting module be connected with the second intelligent processing module respectively and ultrasound wave receiving array.
Second radio receiving transmitting module, gets in touch with beacon module and electronic navigation platform for being responsible for, the sequential of transmitting/receiving wireless synchronizing signal and received ultrasonic signal; Preferably, the second radio receiving transmitting module adopts wireless serial communication module.
Ultrasound wave receiving array, for receiving the ultrasonic signal of the different angles sent from beacon module; Ultrasonic transmission/reception array is the integrated ring array with multiple ultrasonic sensing unit, be all integrated on one piece of circuit board, and position relationship is fixed, and is convenient to calculate, and simplifies procedures.This array not only can test ultrasound wave time of arrival, also can test the angle of ultrasound wave emissive source.In addition also by the comparison of the theoretical code with emissive source, can confirm whether this ultrasound wave coding is that the beacon module needed sends, and also can accurately determine hyperacoustic starting point, can solve the interference problem of ambient sound waves.
Second intelligent processing module, for the treatment of the instruction received, the logical relation of module each several part, recognition coding make the ultrasound data information that synchronous sequence arrangement, storage and analysis receive, and ultrasound data is put into computing unit together together with the electronic chart that electronic navigation platform sends carry out computing, obtain position coordinate data.Second intelligent processing module comprises ultrasonic array processing module and central processing unit, ultrasonic array processing module comprises pre-amplifying module and A/D modular converter, ultrasonic array connects the input end of pre-amplifying module, the output terminal of pre-amplifying module connects the analog input end of A/D modular converter, and the digital output end of A/D modular converter connects central processing unit; Central processing unit is also connected with the second radio receiving transmitting module.Pre-amplifying module is identical with the quantity of ultrasonic sensing unit with the number of channels of A/D modular converter, namely the ultrasonic signal that each ultrasonic sensing unit receives all is processed by respective pre-amplifying module and A/D modular converter, finally inputs to central processing unit and carries out focusing on calculating.
The pre-amplifying module of the present embodiment have employed the amplifier of band AGC function, and AGC is automatic growth control.When the signal that ultrasound wave receiving array receives is more weak, AGC amplifier can increase signal amplification factor automatically, weak signal amplitude is strengthened.When the signal that ultrasound wave receiving array receives is stronger, AGC amplifier can reduce signal amplification factor automatically, and to realize, no matter signal is stronger or more weak, and the output signal of pre-amplifying module all keeps relative stability.
A/D modular converter is used for converting the ultrasound wave simulating signal in detection time section to digital signal continuously (when the ultrasonic signal of needs does not arrive, ultrasonic amplitude is close to zero, during arrival, ultrasonic amplitude will in scope of design), and stored in RAM, become a data group; The ultrasonic signal amplitude arrived by central processing unit scanning collection, and process this group ultrasound data; According to the average amplitude of the ultrasonic pulse group received, calculate the threshold value for judging.The present embodiment, by multiple peak-peak ultrasonic signals of the ultrasonic digital signal after conversion, gets its mean value, then with the value of the proper ratio of this mean value as decision threshold.
The ultrasonic signal amplitude that scanning collection arrives, the point reaching judgment threshold then thinks that the ultrasonic signal of current test arrives starting point, otherwise thinks noise signal.Thus realize ultrasound examination, solve the attenuation problem of ultrasonic signal in transmitting procedure.
Particularly, electronic navigation platform is for storing the coding of whole robot, build electronic chart, planning is the moving target of calculating robot or the position of path and beacon module also, makes robot can both obtain the alternate covering of beacon module in the range of movement allowed; And electronic chart is wirelessly passed to the scheduling that robot localization terminal carries out robot; Synchronous also for coordinated beacon module and robot localization terminal, receives the position coordinate data uploaded from each robot localization terminal and shows.Electronic navigation platform specifically comprises:
Carrying the computing machine of navigation software, for storing the coding of whole robot, building electronic chart, as required dispatch robot, control multiple stage robot and complete respective work simultaneously.Each robot all has global unique encodings, the coding of the robot used all is carried the calculator memory storage of navigation software.Send the electronic chart planned to each robot again.
3rd radio receiving transmitting module, for transmitting electronic chart to robot localization terminal, and send positioning synchronous signal, the sequential of coordinated beacon module and robot localization terminal, receive position coordinate data that each robot localization terminal uploads and the announcement information that barrier scanner is uploaded.
Display, for showing the navigation information on electronic chart.
Electronic chart computing module, for planning and the moving target of calculating robot or the position of path and beacon module, generates navigation information.
As shown in Figure 1: the top of ultrasound wave receiving array is equipped with barrier scanner 4; Barrier scanner 4 comprises synchronous rotation system and control system; Synchronous rotation system comprises the support 41 be located on ultrasound wave receiving array, support 41 is installed servomotor 42, servomotor 42 connects compound sensor seat 44 by rotating boarded head 43, compound sensor seat 44 is provided with human body sensor 441 and ultrasonic transmission/reception sensor 442, the lower end of servomotor 42 connects absolute encoder 45; Control system comprises motor drive module 46 and barrier scanning main control module 47, the output terminal of barrier scanning main control module 47 connects motor drive module 46, the input end of barrier scanning main control module 47 connects absolute encoder 45, human body sensor 441 and ultrasonic transmission/reception sensor 442 respectively, and the output terminal of motor drive module 46 connects servomotor 42; Ultrasonic transmission/reception sensor 442 also scans main control module 47 by ultrasonic signal amplifier and barrier and is connected, and the ultrasonic signal that ultrasonic signal amplifier receives for ultrasonic transmission/reception sensor 442 and sends amplifies.The front end of ultrasonic transmission/reception sensor 442 is provided with sound-absorbing pipe 443.The angle that sound-absorbing pipe 443 receives for narrowing ultrasound wave; Carry out detecting with when scanning at use ultrasound wave to barrier, hyperacoustic energy is diffusion, and the angle that sound-absorbing pipe 443 receives by narrowing ultrasound wave by hyperacoustic concentration of energy, thus improves the effect of detection better.Barrier scanning main control module 47 connects electronic navigation platform and robot localization terminal respectively by CAN.
The air navigation aid of the present embodiment, specifically comprises the following steps:
(1) the origin coordinates data of being walked according to moving body by robot localization terminal, in conjunction with the coordinate that the movement locus that edge behind location is default may reach, calculate at least two beacon module cooked up each coordinate place that may reach and need, sequentially call the beacon module of needs respectively, sent wireless signal by the beacon module of calling and respond; After the beacon module of calling all is responded, robot localization terminal launches wireless application positioning signal to beacon module successively, receive the beacon module of application positioning signal, according to test sequence arrangement to robot localization terminal transmission wireless synchronization signal, beacon module and robot localization terminal is made to coordinate synchronism detection;
Concrete, reserve at the movement locus subscript preset the position coordinates point that robot may reach, when Robot movement locus arrives certain coordinate points, sequentially call at least two beacon module from this coordinate points angle the best; Because each beacon module has certain coverage, by the beacon module position at each coordinate place that may reach of reasonable arrangement, make robot can both have the alternate covering of beacon module in the overall range of motion allowed.Wherein, two beacon module can determine the two-dimensional coordinate of robot, and three beacon module can determine the three-dimensional coordinate of robot, can determine the quantity of beacon module according to the type of electronic chart (plane electronics map and electronic three-dimensional map).By the beacon of calling according to test sequence arrangement to robot localization terminal transmission wireless synchronization signal, receive different beacon module to make robot localization terminal in the different time periods and launch the ultrasonic signal of coming.In addition, when there is multiple robot, if there is the problem of request beacon module conflict, inquire about by electronic navigation platform the beacon module that the robot that working calls whether to be taken by other robot, if take, then notify this robot need interval sometime after action again, after arriving interval time, continue assignment test below, thus reach the object that multiple stage robot completes work separately simultaneously separately.Interval time is set to usually in units of millisecond.Further, when robot departs from default movement locus, still can according in other movement locus be located near robot the beacon module planned realize location automatically, thus avoid robot bias motion track just lost, the problem of cisco unity malfunction;
(2) encoded ultrasonic signal is launched respectively to robot chronologically by beacon module again, the robot localization terminal simultaneously receiving synchronizing signal starts data acquisition time window T, real-time data collection also stores successively, according to the sequential of the ultrasonic signal received, marking the time received respectively is T1,t2.。。,ti, wherein, i is identical with the quantity of called beacon module; Then process calculates the ultrasonic signal data segment that receives of each time period, identify that it is encoded, if receive the ultrasonic signal of many group same-codes, only get the one group of ultrasound wave coded signal arrived at first, reject all the other clutters, then time of the starting T of this group ultrasound wave coded signal finding each time period to arrive at first as calculated1',t2'.。。，Ti’；
Concrete, after starting data acquisition time window T, start continuous acquisition data, be labeled as that time of turn-on data acquisition window acquisition time zero point, the data gathered in time window store successively according to sequential when all storing stored in RAM(), and according to the sequential of the ultrasonic signal received, marking the time received respectively is T1,t2.。。,ti; After time window is closed, the data that robot localization terminal will process in RAM immediately.Wherein, according to the sequential of the ultrasonic signal received, marking the time received respectively is T1,t2.。。,tican be understood as: suppose T=0.15s, according to the sequential of the ultrasonic signal received, the time receiving one group of ultrasonic signal that first beacon module is launched is 0.05s, the time receiving one group of ultrasonic signal that second beacon module is launched is 0.1s, then the time period of 0 ~ 0.05s is labeled as T1, the time period of 0.05s ~ 0.1s is labeled as T2; Each time period only obtains the one group of ultrasound wave coded signal arrived at first, wherein, obtain the one group of ultrasound wave coded signal arrived at first and refer to the ultrasonic signal arrived at first in certain ultrasonic sensing unit or certain several ultrasonic sensing unit, and must be that straight line arrives; All the other clutters comprise the signal that multipath effect, non-line-of-sight propagation and ultrasonic reflections produce, and reject the interference problem that these clutters can solve ambient sound waves;
(3) the distance z between ultrasound wave receiving array and each beacon module is calculatedifor zi=Ti' × c, wherein, c is ultrasonic signal velocity of propagation at normal temperatures, and needs to revise the velocity of propagation c of ultrasonic signal;
(4) the distance L of ultrasound wave receiving array theoretical center and each beacon module is calculatediand the dextrorotation angle γ of beacon module and ultrasound wave receiving array theoretical centeri; Wherein, ultrasound wave receiving array theoretical center refers to the geometric center point of ultrasound wave receiving array; Dextrorotation angle refers to the angle that the distance straight line between this beacon module and ultrasound wave receiving array theoretical center along the axis of ultrasound wave receiving array theoretical center to right rotation is;
(5) ultrasonic signal of two-way or three tunnel beacon module straight lines arrival ultrasound wave receiving arraies is selected out by robot localization terminal, and carry out computing in conjunction with built-in electronic chart, namely receive and dispatch the distance between ultrasound wave receiving array theoretical center and this two-way or three tunnel beacon module calculated according to step (4) respectively, and calculate two-dimensional coordinate data or the three-dimensional coordinate data of the position of robot in conjunction with the stationary coordinate of this two-way or three tunnel beacon module;
(6) two-dimensional coordinate data of gained or three-dimensional coordinate data are wirelessly transmitted to electronic navigation platform, generate the navigation information of robot and show;
(7) when robot motion meets obstacle to the halves, by stop motion, optionally process.If the obstacle run into is people, voice reminder people are given way, as time-out does not allow, then notify electronic navigation platform, by manpower intervention process; If what run into is object, robot is by wireless notification electronic navigation platform, and oneself planning simultaneously can be detoured in the range of movement of oneself, if can walk around, planning is detoured, and as do not walked around, notifies navigation platform, by manpower intervention process.
Concrete, when robot motion meets obstacle to the halves, send instruction to CAN, CAN, after the instruction receiving robot transmission, can pass to barrier scanning main control module 47 instruction, barrier scanning main control module 47 can judge that whether this instruction is effective upon receipt of the instructions: as invalid, then directly abandon, as effectively, will decompose instruction, therefrom get scanning angle information, need the angle (as 45 °) of test according to instruction and need the angular range (as 40 ° ~ 50 °) of scanning, barrier scanning main control module 47 controls motor drive module 46, servomotor 42 (as 40 ° ~ 50 °) in the angular range of instruction is driven to swing, the angle that absolute encoder 45 synchronous feedback coaxial with servomotor 46 swings to, in the process, the angular range of needs to be decomposed into N(before and after this angle within the scope of certain angle as 50 by instruction by barrier scanning main control module) individual point, servomotor is respectively at these somes pause certain hour (as 0.1 second), ultrasonic transmission/reception sensor and human body sensor will be tested in these pause points, get the data (ultrasound wave time of return and intensity) in these points, data in the data of these 50 test points and absolute encoder string successively, namely obtain scan-data, barrier scanning main control module processes scan-data, determines in the angle of specifying, have clear, barrier size and barrier whether to be people.
In addition, if what run into is object for above-mentioned, robot can plan and can to detour in the range of movement of oneself, specifically: the clog-free beacon module that robot has been planned by institute in other movement locus near selecting positions data calculating.
For convenience of describing, the present embodiment is specifically described at indoor moving for robot.As shown in Figure 2: robot localization terminal 2 is fixedly mounted on the top of robot 1, the ultrasound wave receiving array 21 that it carries is arranged on the top center of robot 1 obliquely, its angle of inclination is as the criterion can obtain indoor maximum range of receiving, and ultrasound wave receiving array 21 comprises the ultrasonic sensing unit of several circle distribution.The top of ultrasound wave receiving array 21 is equipped with barrier scanner 4; Also be provided with a set of electronic navigation platform 3.
On the two sides of wall, beacon module A ~ H is installed, the ultrasonic signal that each beacon module A ~ H launches will cover the panel region of oneself, when having 3 tunnels and above signal, then jointly be covered with a panel region, and this overlay area forms crossings on different level formula, robot 1, when indoor moving, all can walk in the clover leaf ultrasound wave overlay area of formation.When robot 1 needs positioning signal, the origin coordinates data of being walked according to robot 1 by robot localization terminal 2, in conjunction with the coordinate that the movement locus that edge behind location is default may reach, calculate three beacon module cooking up the optimum position that each coordinate place that may reach needs, make to be coordinated synchronism detection by the beacon module of calling and robot 1 according to above-mentioned localization method, in the present embodiment, robot 1 moves forward from origin coordinates along movement locus, when moving on the position coordinates preset in somewhere, required beacon module is D, E, F, beacon module D, E, the three-dimensional coordinate of F is respectively D(0, 0, 0), E(0, Y, 0) and F(X, 0, 0).
The beacon module after wireless synchronization signal is launched by the sequential of D, E, F, launch with respective ultrasonic signal of encoding by this sequential to robot localization terminal 2 successively, the robot 1 simultaneously receiving synchronizing signal starts data acquisition, and robot 1 will receive the signal on more than 3 tunnels through carried ultrasonic array 21.As shown in Figure 3: according to reliability needs, select out 3 tunnels for the ultrasonic signal data (ultrasound data only adopting straight line to arrive, rejects the undesired signal because multipath effect and non-line-of-sight propagation cause) calculated, calculate the three-dimensional coordinate (x at robot place, y, z):
Wherein, L1, L2and L3be respectively the distance of ultrasound wave receiving array theoretical center and three beacon module D, F, E.For ease of understanding L1, L2and L3computation process, the ultrasonic signal that the beacon module D that the present embodiment is also (0,0,0) with three-dimensional coordinate launches is example to calculating L1process be specifically described.With reference to shown in Fig. 4 and Fig. 5: ultrasound wave receiving array comprises 16 ultrasonic sensing unit r0-r15, when the ultrasound wave emissive source of the beacon module D of (0,0,0) launches ultrasound wave, received by these five ultrasonic sensing units of the r0-r4 in ultrasound wave receiving array, wherein z0-z4for r0-r4 respectively with the distance of ultrasound wave emissive source, O is ultrasound wave receiving array theoretical center, and R is 16 ultrasonic sensing units of circle distribution and the distance of ultrasound wave receiving array theoretical center, L1for the distance of ultrasound wave emissive source and ultrasound wave receiving array theoretical center, angle b and length s is the fixed measure of ultrasound wave receiving array.
Wherein, at z0-z4in, z3minimum, z2secondary little, the dextrorotation angle of r3 is α, and angle is straight line z2and the angle between length s, ultrasound wave receiving array theoretical center to ultrasound wave emissive source distance L1with nearest ultrasonic sensing unit r3angular deflection be β, now the angle of ultrasound wave receiving array theoretical center and ultrasound wave emissive source is γ1, calculate:
L can be tried to achieve according to above-mentioned formula1value, in like manner can try to achieve L2and L3.Wherein, z0-z4namely ultrasonic sensing unit can be obtained by the step (1) in above-mentioned air navigation aid-step (3) with the distance of ultrasound wave emissive source.
Because under outside certain condition, its velocity of sound is certain, when external condition changes, we can revise the velocity of sound according to the knots modification of external condition.Use and calculate through the revised velocity of sound, just after external condition changes, also can calculate accurate distance.
Coordinate due to each beacon module is fixing, then the distance (absolute distance) between them is also fixing.Any one in ultrasound wave effective coverage range is local, first under fixing acquiescence sonic conditions, can calculate relative distance and the angle of ultrasound wave receiving array and two beacon module; Relative distance (under acquiescence sonic conditions) between the beacon module calculating two stationary coordinate again, compared by the relative distance between the beacon module of these two stationary coordinate and the absolute distance between them again, just can calculate the accurate current velocity of sound.
For ease of understanding, the present embodiment get three-dimensional coordinate for the beacon module F of (X, 0,0) and the beacon module D of (0,0,0) be that example is specifically described, with reference to shown in Fig. 6:
D1for the absolute distance (in units of mm) between beacon module F and beacon module D, acquiescence velocity of sound c1, current real velocity of sound is c2, L1, L2, dextrorotation angle γ1and γ2calculate in above-mentioned formula, thus the angle μ=γ between the beacon module obtaining two stationary coordinate2-γ1, the relative distance d between the beacon module of two stationary coordinate2for:
Due to d1=c2t, d2=c1t, therefore real velocity of sound formula is:
By calculating percentage error between absolute distance between two beacon module and relative distance as compensation, and then calculate the accurate velocity of sound under current environmental condition.
Above-mentioned computation process is realized by the central processing unit in robot localization terminal 2, by robot 1, the computing module that the ultrasonic signal received puts into central processing unit together together with electronic chart is calculated, by comparing the phase differential of the ultrasonic signal received, judge the angle of robot 1 in electronic chart, the three-dimensional coordinate data of this position gained is wirelessly transmitted to electronic navigation platform 3 the most at last, generates the navigation information of robot 1 and shows.
In the walking process of robot 1, only display mobile route navigation, not uploading position coordinate data, only has the coordinate place arriving and preset just to electronic navigation platform 3 uploading position coordinate data, as in Fig. 1, when robot 1 walks to when taking beacon module A, B, C as required position coordinates place of calling, calculate the three-dimensional coordinate of robot according to said method, be uploaded to electronic navigation platform 3.When robot 1 runs into barrier in the process of walking, as the scanner 4 that breaks the barriers detect that the barrier run into is object time, robot 1 will notify electronic navigation platform 3, automatically plan that the range of movement of oneself can detour simultaneously, such as select clog-free beacon module D, F, G to position data calculating, plan and detour.