CN105606101B - A kind of robot indoor navigation method based on ultrasonic measurement - Google Patents
A kind of robot indoor navigation method based on ultrasonic measurement Download PDFInfo
- Publication number
- CN105606101B CN105606101B CN201510963172.3A CN201510963172A CN105606101B CN 105606101 B CN105606101 B CN 105606101B CN 201510963172 A CN201510963172 A CN 201510963172A CN 105606101 B CN105606101 B CN 105606101B
- Authority
- CN
- China
- Prior art keywords
- robot
- width
- ultrasonic wave
- running orbit
- moment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
- G01S15/06—Systems determining the position data of a target
- G01S15/08—Systems for measuring distance only
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Acoustics & Sound (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
The present invention provides a kind of robot indoor navigation method based on ultrasonic measurement, is applied in indoor verandas comprising following steps：Two ultrasonic wave modules are individually fixed on the medianly zygomorphic position of robot, and the normal direction of two ultrasonic wave modules and the angle on ground are；Set running orbit of the robot indoors on corridor, robot operation；The actual range and width of corridor between robot and side walls are obtained respectively；The above-mentioned data obtained according to two ultrasonic wave modules carry out the variable quantity that analysis calculates Robot track normal direction position deviation under different situations；Then the revised deviation angle of calculating robot, and according to the running orbit of deviation angle adjustment robot, the current running orbit for being is overlapped with setting running orbit.The present invention is modified the course angle information of robot, improves the reliability of robot long-play.
Description
Technical field
The present invention relates to Navigation Control technical field more particularly to a kind of robot indoor navigations based on ultrasonic measurement
Method.
Background technology
Indoors in environment, the most basic operational mode of mobile robot is exactly to carry out straight line traveling along corridor, due to soft
Hardware aspect various factors (the electric machine speed regulation characteristic of such as driving wheel is inconsistent, wheel installation error, ground friction coefficient not
Same, controlled quentity controlled variable resolution error, navigational parameter resolution error etc.), cause robot that can deviate setting in being travelled along straight path
Track can not complete expectation function if without correction.And reference information important during straight line is rectified a deviation is
The positioning result of vehicle.
Using inertial navigation to robot carry out positioning calculation, by a large amount of robot straight line rectify a deviation experimental result it is found that
Robot location's information of dead reckoning precision on the direction of straight-line travelling is higher, and is resolved on traveling direction method line direction
As a result larger with deviations of actual position, this error is mainly derived from inertial reference calculation the course angle of error accumulation at any time.Cause
This should be robot navigation's control system and is introduced into measurement to available information in environment, thereby using external information measurement result
Inertial navigation is corrected.
Invention content
The present invention provides a kind of robot indoor navigation method based on ultrasonic measurement, passes through the course angle to robot
Information is modified, and improves the reliability of robot long-play.
The present invention is achieved through the following technical solutions：
A kind of robot indoor navigation method based on ultrasonic measurement is applied in indoor verandas comprising following step
Suddenly：
Step 1, two ultrasonic wave modules are individually fixed on the medianly zygomorphic position of robot, and two ultrasounds
The normal direction of wave module and the angle on ground are θ；
Step 2, the setting robot running orbit on corridor indoors, robot operation；
The k moment, according toThe actual range D between robot and side walls is obtained respectivelyL
(k) and DR(k)；Wherein, DL is surveyed(k)、DR is surveyed(k) ultrasonic wave that respectively two ultrasonic wave modules in left and right measure in the normal direction
Module is at a distance between the walls；
According to Width (k)=DL(k)+DR(k)+W obtains width of corridor Width (k)；Wherein W is described robot or so
Width between monosymmetric position；
Step 3, at the k moment, if course angle changes delta ψ is more than T1, robot continues to run by the running orbit of setting；
If course angle changes delta ψ is less than or equal to T1,4 are thened follow the steps；Wherein, course angle changes delta ψ is that current course is navigated with setting
To difference；
Step 4, by the width of corridor Width (k) of width of corridor result Width (k-1) and current time of last moment
It is poor to make, if difference is more than threshold value T2, thens follow the steps 5；If difference is less than or equal to threshold value T2, basisCalculate k moment Robots track normal direction position deviation
Variation delta Xn(k), step 6 is then executed；
Wherein, DL_tempAnd DR_tempFor history distance measurement result, pass through the ranging knot of two ultrasonic wave modules of preceding k-1 moment
Obtained by fruit is averaged respectively；
Step 5, by DL(k) with history distance measurement result DL_tempIt is poor make, by DR(k) with history distance measurement result DR_tempInto
It is poor that row is made, if only there are one differences more than T3, according to enabling Δ Xn(k) then the difference equal to less than T3 executes step 6；
If two differences are all higher than T3, robot continues to run by the running orbit of setting；
If two differences are respectively less than or are equal to T3, basis
Calculate k moment Robots track normal direction position
Set the variation delta X of deviationn(k), step 6 is then executed；
Step 6, according toThe revised deviation angle ψ of calculating robotUS, and
According to deviation angle ψUSAdjust the running orbit of robot so that current running orbit is overlapped with setting running orbit；Wherein, Xt
(k) and Xt (k-1) is that adjacent course angle twice resolves the Robot orbit tangent direction position at moment as a result, being solved by DR
It calculates and obtains.
Further, the θ is 10 degree.
Further, the threshold value T1 is 5 °, and threshold value T2 is 4cm, and threshold value T3 is 3cm.
Compared with prior art, the beneficial effects of the invention are as follows：
By the amendment of the algorithm, the course angle of robot by constantly correct can maintain in a long time it is higher
Precision, it is ensured that the stable operation of system.So far the design of robot inertia/ultrasonic wave Integrated Navigation Algorithm is completed.
Description of the drawings
Fig. 1 is ultrasonic wave module installation and the ranging schematic diagram of the present invention；
Fig. 2 is the ultrasonic ranging result distinguishing validity flow chart of the present invention；
Fig. 3 is that width of corridor and course angle schematic diagram are asked in the ultrasonic ranging of the present invention；
Fig. 4 is the present invention using ultrasonic ranging modified result robot course angle method.
Specific implementation mode
In order to make the purpose , technical scheme and advantage of the present invention be clearer, with reference to the accompanying drawings and embodiments, right
The present invention is further elaborated.It should be appreciated that the specific embodiments described herein are merely illustrative of the present invention, and
It is not used in the restriction present invention.
As shown in Fig. 2, a kind of robot indoor navigation method based on ultrasonic measurement comprising following steps：
Step 1, as shown in Figure 1, two ultrasonic wave modules are individually fixed on the medianly zygomorphic position of robot,
Module is away from ground 10cm or so.The ultrasonic wave that should be reflected by metope of module transmitting, will in advance by ground return in order to prevent
Ultrasonic wave module tilts upward installation.The stability and precision of different setting angle lower module measurement data are tested, it is final to determine
Setting angle is 10 °.
Step 2, the running orbit on corridor, robot operate in the k moment indoors for setting robot, according toThe actual range D between robot and side walls is obtained respectivelyL(k) and DR(k)；Wherein, DL is surveyed
(k)、DR is surveyed(k) it is respectively the ultrasonic wave module that measures in the normal direction of two ultrasonic wave modules at a distance between the walls.
In real time obtain the current course ψ of robot, and with setting course ψtargetCompare.As the current course ψ of robot and setting
Course ψtargetWhen misaligned, vertical range of the robot far from wall should have
In view of the precision that the complexity and course angle inertia of calculating resolve, heading angle deviation Δ ψ is worked as in setting in a program
=ψ-ψtargetIt when less than 5 °, is directly calculated using formula (1), reduces calculation amount.
According to Width (k)=DL(k)+DR(k)+W obtains width of corridor Width (k)；Wherein W is at left and right sides of robot
Width between symmetrical position；
Step 3, before traveling, robot can be controlled and carry out pivot turn, and constantly acquisition left and right sides ultrasound
It is analyzed away from result, so that it is carved at the beginning automatically along setting running orbit direction, only propose a think of in this application
It examines, robot is set in this programme and is carved at the beginning along setting running orbit direction.As shown in figure 3, former clockwise in robot
During ground rotates, the both sides obstacle distance of measurement is the angle [alpha] of rotationn(between robot normal and setting course
Angle) function, i.e. WLR(αn).If side walls are parallel, there is width of wall body Width to be expressed as：
Width=min (WLR(αn)) (2)
Pass through WLR(αn) corresponding robot rotates clockwise angle [alpha] when being minimized for the first timenIt can obtain rotation
Initial time robot course angle ψ0, i.e. angle of the robot direction of travel relative to corridor direction.Have
The wherein course angle ψ of robot0And rotation angle αnTo clockwise turn to positive direction, and think robot
Initial heading ψ0Angle between the direction of corridor does not exceed 90 °, and ultrasonic wave sample frequency is higher in rotary course, measures knot
The precision of fruit is higher, but is still limited by the range accuracy of ultrasonic wave module.
If at the k moment, course angle changes delta ψ meets | Δ ψ | ＞ T1, then judge that robot is significantly being turned, no
Navigator fix auxiliary is carried out using the distance measurement result of ultrasonic wave to resolve, robot continues to run by the running orbit of setting；Otherwise
Execute step 4；
Step 4, in the case where robot keeps straight on, by the history width of corridor result Width (k-1) of storage with it is current
The width of corridor result Width (k) of resolving is compared, when the two deviation is more than threshold value T2, it is believed that the corridor that vehicle passes through
Saltus step has occurred by parallel continuous state in metope, and executes step 5；If difference is less than or equal to threshold value T2, two super
The distance measurement result of sound wave module is used to auxiliary positioning, and according to
Calculate the variation delta X of k moment Robots track normal direction position deviationn(k), step 6 is then executed；；
Step 5, respectively to the current distance measurement result D of left and right sides ultrasonic wave moduleL(k) and DR(k) with history distance measurement result
DL_tempAnd DR_tempCarry out make difference and compared with T3, i.e.,
Two judgements meet its a period of time, then illustrate to be equipped with sundries, the distance measurement result of the side ultrasonic wave in corridor at the metope
It is unavailable, using only the distance measurement result progress auxiliary positioning of other side ultrasonic wave, and according to
Calculate the variation delta X of k moment Robots track normal direction position deviationn(k), specifically, when left side can be used calculation formula
For DL_temp-DL(k), when right side can be used, calculation formula is DR(k)-DR_temp, then execute step 6；Two judgements are satisfied by,
Then this time data is unavailable, resurveys data；Robot continues to run by the running orbit of setting.Two judging results are equal
It is unsatisfactory for, then illustrates that side walls shape does not change in corridor, the distance measurement result of both sides ultrasonic wave is available；And according toCalculate k moment Robots track normal direction position deviation
Variation delta Xn(k), step 6 is then executed.
Wherein, DL_tempAnd DR_tempFor history distance measurement result, pass through the ranging knot of two ultrasonic wave modules of preceding k-1 moment
Obtained by fruit is averaged respectively.Effect is preferable, if in the measurement of continuous several times resolves, the width Width of wall, both sides
The distance measurement result D of ultrasonic waveLAnd DRBig change does not occur, then is deposited into buffer area, is walked respectively as the history of subsequent time
Corridor width result Width (k-1) and ultrasonic wave history distance measurement result DL_tempAnd DR_temp。
By experiment repeatedly, threshold value T1 is determined as 5 °, and threshold value T2 is determined as 4cm, and threshold value T3 is determined as 3cm.
In conclusion in kth time positioning calculation, according to this result DL(k)、DR(k) and historical data DL_temp、DR_temp
Extrapolate the variation delta X of k moment Robots track normal direction position deviationn, have
After the completion of kth time resolves, historical data D is updatedL_temp、DR_tempFor kth time measurement result DL、DR。
Step 6, according toThe revised deviation angle of calculating robot.In formula,
ψUSIt is the course angle solved with ultrasonic ranging result, Δ Xn (k) and Δ Xn (k-1) are respectively adjacent ultrasonic wave course angle twice
The robot Norma l deviation at moment is resolved as a result, the result has already been through previously described method is modified；Xt (k) and Xt
(k-1) it is that the adjacent course angle of ultrasonic wave twice resolves the Robot orbit tangent direction position at moment as a result, the information is logical
It crosses DR and resolves acquisition.As shown in figure 4, according to deviation angle ψUSAdjust the running orbit of robot.
During storing ultrasonic ranging data, need to continue to judge whether ultrasound data can be used, and if only if
When the result that continuous 15 ultrasonic waves resolve is effective, just resolved；Otherwise, ultrasound data memory buffers area is emptied, weight
The new timing for carrying out resolving the period.
As the course angle ψ that the ultrasonic ranging result of storage resolvesUSWith the course angle ψ of inertial reference calculationINSDifference be more than threshold value
When Threshold_ ψ, course angle ψ is resolved using ultrasonic waveUSUpdate inertial reference calculation result ψINS.By the amendment of the algorithm, machine
The course angle of people can maintain higher precision in a long time by constantly correcting, it is ensured that the stable operation of system.Extremely
This completes the design of robot inertia/ultrasonic wave Integrated Navigation Algorithm.
The foregoing is merely illustrative of the preferred embodiments of the present invention, is not intended to limit the invention, all essences in the present invention
All any modification, equivalent and improvement etc., should all be included in the protection scope of the present invention made by within refreshing and principle.
Claims (3)
1. a kind of robot indoor navigation method based on ultrasonic measurement is applied in indoor verandas, which is characterized in that including
Following steps：
Step 1, two ultrasonic wave modules are individually fixed on the medianly zygomorphic position of robot, and two ultrasonic wave moulds
The normal direction of block and the angle on ground are θ；
Step 2, the setting robot running orbit on corridor indoors, robot operation；
The k moment, according toThe actual range D between robot and side walls is obtained respectivelyL(k)
And DR(k)；Wherein, DL is surveyed(k)、DR is surveyed(k) ultrasonic wave module that respectively two ultrasonic wave modules in left and right measure in the normal direction
At a distance between the walls；
According to Width (k)=DL(k)+DR(k)+W obtains width of corridor Width (k)；Wherein W is at left and right sides of the robot
Width between symmetrical position；
Step 3, at the k moment, if course angle changes delta ψ is more than T1, robot continues to run by the running orbit of setting；If boat
It is less than or equal to T1 to angle changes delta ψ, thens follow the steps 4；Wherein, course angle changes delta ψ is current course and setting course
Difference；
Step 4, it is poor the width of corridor result Width (k-1) of last moment and the width of corridor Width (k) at current time to be made,
If difference is more than threshold value T2,5 are thened follow the steps；If difference is less than or equal to threshold value T2, basisCalculate k moment Robots track normal direction position deviation
Variation delta Xn(k), step 6 is then executed；
Wherein, DL_tempAnd DR_tempFor history distance measurement result, pass through the distance measurement result point of preceding two ultrasonic wave modules of k-1 moment
Obtained by not averaging；
Step 5, by DL(k) with history distance measurement result DL_tempIt is poor make, by DR(k) with history distance measurement result DR_tempMade
Difference, if only there are one differences to be more than T3, according to enabling Δ Xn(k) then the difference equal to less than T3 executes step 6；
If two differences are all higher than T3, robot continues to run by the running orbit of setting；
If two differences are respectively less than or are equal to T3, basis
It is inclined to calculate k moment Robots track normal direction position
The variation delta X of differencen(k), step 6 is then executed；
Step 6, according toThe revised deviation angle ψ of calculating robotUS, and according to this
Deviation angle ψUSAdjust the running orbit of robot so that current running orbit is overlapped with setting running orbit；Wherein, Xt (k) and
Xt (k-1) is that adjacent course angle twice resolves the Robot orbit tangent direction position at moment as a result, being obtained by DR resolvings
.
2. the robot indoor navigation method based on ultrasonic measurement as described in claim 1, which is characterized in that θ is 10 degree.
3. the robot indoor navigation method based on ultrasonic measurement as described in claim 1, which is characterized in that threshold value T1 is
5 °, threshold value T2 is 4cm, and threshold value T3 is 3cm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510963172.3A CN105606101B (en) | 2015-12-21 | 2015-12-21 | A kind of robot indoor navigation method based on ultrasonic measurement |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510963172.3A CN105606101B (en) | 2015-12-21 | 2015-12-21 | A kind of robot indoor navigation method based on ultrasonic measurement |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105606101A CN105606101A (en) | 2016-05-25 |
| CN105606101B true CN105606101B (en) | 2018-07-17 |
Family
ID=55986232
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510963172.3A Active CN105606101B (en) | 2015-12-21 | 2015-12-21 | A kind of robot indoor navigation method based on ultrasonic measurement |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105606101B (en) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| TWI634403B (en) * | 2017-01-26 | 2018-09-01 | 好樣科技有限公司 | An automatic cleaning robot and a controlling method of the same |
| CN107571257B (en) * | 2017-08-04 | 2020-05-19 | 广东美的智能机器人有限公司 | Robot control method, robot control system and robot |
| CN109000648A (en) * | 2018-05-03 | 2018-12-14 | 南京理工大学 | A kind of mobile robot indoor orientation method based on laser filtering |
| CN109283544A (en) * | 2018-10-06 | 2019-01-29 | 中国航空工业集团公司洛阳电光设备研究所 | A kind of autonomous charging of robots alignment methods based on laser ranging |
| CN109798901B (en) * | 2019-03-18 | 2022-08-12 | 国网江苏省电力有限公司电力科学研究院 | Robot for files and navigation positioning system and navigation positioning method thereof |
| CN110926492B (en) * | 2019-12-09 | 2024-03-15 | 山西航天清华装备有限责任公司 | Vehicle short-distance navigation method |
| CN113029200A (en) * | 2021-03-29 | 2021-06-25 | 上海景吾智能科技有限公司 | Method, system and medium for testing course angle and accuracy based on robot sensor |
| CN114577206B (en) * | 2022-03-08 | 2023-10-31 | 宁波诺丁汉大学 | Indoor inertial navigation map building method and system based on ultrasonic waves |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102436260A (en) * | 2011-09-01 | 2012-05-02 | 北京航空航天大学 | Indoor self-positioning and self-directing two-dimensional navigation system |
| CN103487812A (en) * | 2013-08-30 | 2014-01-01 | 江苏大学 | Ultrasonic navigation unit of greenhouse automatic moving vehicle and method |
| CN104020447A (en) * | 2014-05-27 | 2014-09-03 | 美新半导体（无锡）有限公司 | Indoor combined positioning system and positioning method thereof |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100772912B1 (en) * | 2006-05-16 | 2007-11-05 | 삼성전자주식회사 | Robot using absolute azimuth and method for mapping by the robot |
| US8874261B2 (en) * | 2007-07-25 | 2014-10-28 | Deere & Company | Method and system for controlling a mobile robot |
-
2015
- 2015-12-21 CN CN201510963172.3A patent/CN105606101B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102436260A (en) * | 2011-09-01 | 2012-05-02 | 北京航空航天大学 | Indoor self-positioning and self-directing two-dimensional navigation system |
| CN103487812A (en) * | 2013-08-30 | 2014-01-01 | 江苏大学 | Ultrasonic navigation unit of greenhouse automatic moving vehicle and method |
| CN104020447A (en) * | 2014-05-27 | 2014-09-03 | 美新半导体（无锡）有限公司 | Indoor combined positioning system and positioning method thereof |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105606101A (en) | 2016-05-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105606101B (en) | A kind of robot indoor navigation method based on ultrasonic measurement | |
| EP3552532B1 (en) | Method for angle correction of mobile robot in working area and mobile robot | |
| US9740209B2 (en) | Autonomous moving body | |
| CN107618503B (en) | A kind of automatic parking control method and system | |
| US10145690B2 (en) | Map information generation system, method, and program | |
| US8538671B2 (en) | Apparatus and method for detecting position and orientation of mobile object | |
| Goel et al. | Robust localization using relative and absolute position estimates | |
| US10860035B2 (en) | Travel history storage method, method for producing travel path model, method for estimating local position, and travel history storage device | |
| US20160107682A1 (en) | System and method for vehicle steering control | |
| US20070271003A1 (en) | Robot using absolute azimuth and mapping method thereof | |
| CN111060135A (en) | Map correction method and system based on local map | |
| JP2002215236A (en) | Controller for travel of unmanned vehicle | |
| JP2001515237A (en) | Docking method of autonomous motion unit using guidance beam | |
| WO2019037618A1 (en) | Route planning-based vehicle motion control method and apparatus, and related device | |
| CN104864867B (en) | GNSS vehicle is applicable in VSYR blind areas position error modification method | |
| US20160231432A1 (en) | Method for determining a current position of a motor vehicle in a geodetic coordinate system and motor vehicle | |
| US11579628B2 (en) | Method for localizing a vehicle | |
| JP7133251B2 (en) | Information processing device and mobile robot | |
| CN105752154A (en) | Vehicle steering control system and method | |
| JP6539129B2 (en) | Vehicle position estimation device, steering control device using the same, and vehicle position estimation method | |
| JP2020004342A (en) | Mobile body controller | |
| Sliwka et al. | Sea glider guidance around a circle using distance measurements to a drifting acoustic source | |
| JP2017111771A (en) | Autonomous travel vehicle, autonomous travel system and control method of autonomous travel vehicle | |
| JP5074153B2 (en) | Route generation device and method, and mobile device including route generation device | |
| JP7040308B2 (en) | Travel control device and travel control method for automatic guided vehicles |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |