CN110231627A - Service robot operating path calculation method based on visible light-seeking - Google Patents
Service robot operating path calculation method based on visible light-seeking Download PDFInfo
- Publication number
- CN110231627A CN110231627A CN201910664173.6A CN201910664173A CN110231627A CN 110231627 A CN110231627 A CN 110231627A CN 201910664173 A CN201910664173 A CN 201910664173A CN 110231627 A CN110231627 A CN 110231627A
- Authority
- CN
- China
- Prior art keywords
- service robot
- algorithm
- node
- visible light
- calculation method
- 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.)
- Pending
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/46—Indirect determination of position data
- G01S17/48—Active triangulation systems, i.e. using the transmission and reflection of electromagnetic waves other than radio waves
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
Abstract
The service robot operating path calculation method based on visible light-seeking that the present invention relates to a kind of, belongs to intelligent robot control field.The path calculation method carries out evading for path planning and barrier using A-star algorithm；Dual ranging calculating is carried out for practical ranging or prediction ranging using RHLED-RSS location algorithm and PDR inertial navigation location algorithm；The path calculation method uses transmitting terminal of the LED light source as signal, and modulated optical signal is sent out while providing illumination；The path computing side obtains the real time position of service robot by RHLED-RSS location algorithm, PDR inertial navigation location algorithm, then the shortest path of designated place is reached by A-star algorithmic rule robot.The present invention uses transmitting terminal of the light source as signal, and modulated optical signal is sent out while providing illumination, demodulation calculating is carried out in robot signal receiving end, to realize positioning function.Visible light has the advantages of low energy consumption, high-speed, no electromagnetic interference relative to traditional communication technology.
Description
Technical field
The service robot operating path calculation method based on visible light-seeking that the present invention relates to a kind of, belongs to robot intelligence
It can control field.
Background technique
In recent years, with the rapid development of wireless sensor network and technology of Internet of things, indoor positioning technologies are in intelligent machine
It is widely applied in the numerous areas such as device people, megastore shopping guide, becomes one of primary study object.Past several years
In, the research of indoor positioning is predominantly stayed in using technologies such as GPS, RFID, infrared ray, ultrasonic wave, WLAN.However, using
When GPS signal carries out indoor positioning, radiofrequency signal after penetrating building walls is very weak to cause position error excessive.Another party
Face, using RFID, infrared ray, ultrasonic wave when the technological means such as WLAN position, needs to build complicated location facilities environment, no
Only at high cost, positioning accuracy and safety also cannot get effective guarantee.The invention of LED is that lighting technical field brings new leather
Life.LED almost combines the advantage of various conventional light sources, has low using voltage, power is low, the service life is long, is easy to minimize etc.
Advantage.White light LEDs have High Speed Modulation and the characteristics such as the response time is short simultaneously, so that the application of LED be made to extend from lighting area
The communications field has been arrived, can be achieved at the same time illumination and communication dual function.Indoor visible light communication (VLC) based on white light LEDs
As a kind of emerging communication, electromagnetic radiation, use environment, in terms of and Radio-Frequency Wireless Communication mode
Compared to there is apparent advantage.Realize that indoor positioning is considered as effectively selecting using visible light for these reasons.In the recent period, may be used
Light-exposed location technology has obtained more in-depth study.Visible light communication technology rapid development, relative to traditional communication technology
(such as bluetooth and Wi-Fi), with low energy consumption, high-speed, without remarkable advantages such as electromagnetic interferences.
Yoshino.M etc. has gone out a kind of visible light positioning system based on visible LED light imaging sensor, may be implemented to survey
The position and direction of receiving end are measured, wherein each LED in illumination sends difference three dimensional space coordinate, and imaging sensor can
To receive signal, but need additional image processing techniques and data rate is limited to image procossing speed.
Zhang Weizhi etc. has gone out a kind of positioning for receiving and dispatching difference power estimation transmission range by OOK modulated signal using 4 LED
Scheme, although can obtain certain precision needs being affected for modulation technique and bias light.
The location algorithm of the propositions such as Vegni A M is the thought based on fingerprint location, needs big training set, calculation amount
Greatly.Common visible light location technology is mainly based upon triangulation method, and this method needs to estimate the distance of transmitting terminal and receiving end.
Transmission range information is introduced location algorithm by the feature high apart from short positioning accuracy from transmission of visible light
In.There are many methods that ranging may be implemented and such as receives letter.Number intensity (RSS), arrival time (TOA), reaching time-difference (TDOA),
Angle of arrival (AOA) etc..AOA technology can achieve good precision estimation, but need to dispose imaging sensor in receiver-side
Array, this is very expensive.For indoor environment, when the relatively short distance between transmitter and receiver leads to the transmission of signal
Between it is very short, to transmitter and the clock accuracy of receiver and it is synchronous require it is very high so that TOA and TDOA technology is difficult to realize.
Kim H etc. proposes a kind of visible light indoor locating system using RSS technology, which uses and only with 3
A LED emits different carrier signals as transmitter, and receiving end is according to the signal strength estimation target and transmitter received
Distance, and obtain the positioning performance that error is about 6cm.Positioning node (PN) is caused to exist however, 3 positioned light sources are used only
The decline of edges of regions positioning accuracy.
Ganti D etc. proposes a kind of simple indoor orientation method based on VLC, and to the Kalman for realizing tracking prediction
Carried out simulation analysis with the performance of particle filter algorithm, but this tracking performance adjust the distance estimate initial value required precision compared with
Height, and distance estimations use in algorithm its precision of traditional trilateration and anti-block ability and be all difficult to meet the requirements.
Summary of the invention
The present invention provides a kind of service robot operating path calculating side based on visible light-seeking in view of the above deficiencies
Method.
The present invention adopts the following technical scheme:
Service robot operating path calculation method of the present invention based on visible light-seeking, the path calculation method
Evading for path planning and barrier is carried out using A-star algorithm；It is calculated using RHLED-RSS location algorithm and PDR inertial navigation positioning
Method carries out dual ranging calculating for practical ranging or prediction ranging；
The path calculation method uses transmitting terminal of the LED light source as signal, and modulation is sent out while providing illumination
Optical signal afterwards；Auxiliary inertial navigation positioning is carried out using PDR inertial navigation location algorithm；The PDR inertial navigation that service robot uses, which positions, calculates
Optical auxiliary signal carries out action positioning when method is directed to relative displacement；
The path computing side obtains the real-time of service robot by RHLED-RSS location algorithm, PDR inertial navigation location algorithm
Position, then the shortest path by A-star algorithmic rule robot arrival designated place；
The service robot built-in infrared obstacle range finder module, infrared obstacle range finder module detect barrier；It is logical
It crosses the detection information of infrared obstacle range finder module feedback and evading for path planning and barrier is carried out by A-star algorithm.
Service robot operating path calculation method of the present invention based on visible light-seeking, the A-star are calculated
Method selects suitable heuristic function using heuristic search mode,
It is calculated and is assessed for the cost value of each expanding node, evaluation function is calculated by following formula:
F (n)=g (n)+h (n)
In formula, f (n) is evaluation function, is the optimal estimation value that starting point passes through that node n reaches home；G (n) is from initial
The actual consumption of node-to-node n；H (n) is the estimation consumption from node n to target point；
Evaluation function obtained by calculation is calculate by the following formula out the heuristic function of map planning:
H (n)=| Xi-Xj|+|Yi-Yj|
X in formulai, YiAnd Xj, YjIndicate the two-dimensional coordinate of position i and position j in map.
Service robot operating path calculation method of the present invention based on visible light-seeking, the A-star are calculated
The state of openlist list and closelist list records node is used in method；
Openlist list is used to record to calculate the node not extended by A-star algorithm；
Closelist list is for recording the node extended；
A node evaluation function f (n) obtained by calculation is applied to openlist list in A-star algorithm, should
Openlist list is ranked up for each node evaluation function f (n) value size, deletes the smallest node of f (n) value, and add
It is extended into closelist list and is circuited sequentially, until searching destination node.
Service robot operating path calculation method of the present invention based on visible light-seeking, the PDR inertial navigation
Location algorithm obtains measurement vector from the sensor in service robot, which is speedometer, gravitometer, magnetometer, top
Spiral shell instrument；
PDR inertial navigation positioning calculation establishes initial time machine and is located at position s0, and establish coordinate in location coordinate and be
(x0, y0)；Service robot shifting then updates coordinate after moving a step be (x1, y1), by following equation calculation service robot in room
Interior square coordinate:
L in formula1For the distance of movement, θ is transported1To move deflection, i is step number.
Service robot operating path calculation method of the present invention based on visible light-seeking, the RHLED-
Coordinate (the x of service robot is established in RSS location algorithmr, yr, zr), and set transmitting terminal TxiDistance be respectively (dc1, dc2,
dc3)；The position coordinates of service robot are obtained by following equation:
R is the point in three-dimensional space in formula.
Service robot operating path calculation method of the present invention based on visible light-seeking, the LED light source
It is 7,7 LED light sources are arranged at regular hexagon shape.
Beneficial effect
Service robot operating path calculation method provided by the invention based on visible light-seeking, using light source as letter
Number transmitting terminal, modulated optical signal is sent out while illumination is provided, robot signal receiving end carry out demodulation
It calculates, to realize positioning function.Visible light has low energy consumption, high-speed, no electromagnetic interference relative to traditional communication technology
The advantages of.
Service robot operating path calculation method provided by the invention based on visible light-seeking, the visible light of use can
Indoor precise positioning is realized based on indoor intensity of illumination, and the positioning side that the positioning accuracy within 10cm is target may be implemented
Method.
Service robot operating path calculation method provided by the invention based on visible light-seeking, be suitable for article it is more and
Miscellaneous, the changeable environment of personage is formulated the operation reserve of obstacle avoidance by specific aim.
Detailed description of the invention
Fig. 1 is algorithm principle frame route map of the invention；
Fig. 2 is triangulation algorithm distance measuring method comparison sheet of the present invention；
Fig. 3 is RHLED-RSS practical layout schematic diagram of the present invention；
Fig. 4 is inventive algorithm structural block diagram schematic diagram；
Fig. 5 is the infrared obstacle range finder module operation principle schematic diagram of the present invention；
Fig. 6 is the SCM program design flow diagram of the infrared obstacle range finder module of the present invention；
Fig. 7 is A-Star algorithm flow chart of the present invention；
Fig. 8 is RHLED-RSS of the present invention and the dual range accuracy assurance model schematic diagram of PDR.
Specific embodiment
To keep purpose and the technical solution of the embodiment of the present invention clearer, below in conjunction with the attached of the embodiment of the present invention
Figure, is clearly and completely described the technical solution of the embodiment of the present invention.Obviously, described embodiment is of the invention
A part of the embodiment, instead of all the embodiments.Based on described the embodiment of the present invention, those of ordinary skill in the art
Every other embodiment obtained, shall fall within the protection scope of the present invention under the premise of being not necessarily to creative work.
Service robot operating path calculation method of the invention based on visible light-seeking as shown in Figure 1:, in software and
Under hardware system collaborative configuration environment, service robot independently will complete path planning by environment indoors, collision prevention and scheduled
Job task, be based on this, first have to complete service robot autonomous positioning, compared with outdoor positioning, indoor positioning environment compared with
Small, barrier is more, therefore uses the visible light-seeking based on visible light high rate communication technology, realizes the high-precision of positioning.
Using A-star algorithm and the static indoor path planning that drives in the wrong direction, the location information of the barrier in indoor environment is known in advance, is mentioned
High service-delivery machine task efficiency.Visible light positioning service robot is in warehouse positioning, underground mine positioning, market, underground
The sphere of life such as parking lot, hospital all have high application scenarios.
(1) visible light location technology, accurate autonomous positioning are based on
The characteristics of according to indoor environment, indoor positioning scheme needs to have the characteristics that precision is high, stability is strong, from positioning accurate
Degree, system cost and electromagnetic radiation etc. consider, it is seen that light-seeking is based on visible light communication technology, using multiple 200Hz
The white light LEDs of frequency provide as transmitting terminal and are sent out the optical signal through ovennodulation while illumination, to realize that information passes
It is defeated, it can be achieved that the precision of service robot autonomous positioning is reduced to Centimeter Level and the cost of system can be greatly reduced；Because being more than
200Hz frequency be harmless, and without electromagnetic radiation can avoid to human health generate harm.
(2) it is based on A-star algorithm, path planning is carried out and obstacle avoids
Indoor environment is typical static environment, the A-Star algorithm based on heuristic search, by indoor map into
Row pretreatment, optimizes feasible zone, improves to A-Star algorithm traversal mode, improved A-Star algorithm can be very
Be applied to the planning in static indoor path well, can greatly improve arithmetic speed of the A-Star algorithm indoors in navigator fix and
Efficiency, distinguishing feature are the obstacle informations known in environment in advance, significantly improve service-delivery machine task efficiency.
(3) reliable service in real time is provided
Visible light positioning service robot carries out autonomous positioning with improved RSS algorithm and improved A-star is combined to calculate
Method carries out path planning and obstacle avoids, in actual scene, so its algorithm time delay determines that can robot provide in real time
Reliable service, and time delay can be significantly reduced in improved algorithm, provides faster more reliable service.
Service robot operating path calculation method based on visible light-seeking of the invention, studying high-precision interior can
Light-exposed location algorithm is improved based on existing LED triangle polyester fibre algorithm, is studied and is arranged based on 7 LED regular hexagon for being one group
The regular hexagon visible light of mode for cloth receives signal strength ranging localization algorithm (RHLED-RSS, Regular Hexagon
Light Emitting Diode-Received Signal Strength)。
B. research indoor visible light positioning application
Service robot and indoor visible light-seeking application can be deployed in based on the research of visible light location algorithm.It includes
Corresponding location hardware equipment (signal projector, signal receiver, data transmission channel, visualization device etc.), data processing
Software etc..
The intelligent method for searching of indoor service robot provided by the invention is specific as follows:
1) service robot inertial navigation location technology is studied
Visible light location algorithm is assisted to carry out robotic movement by the relative displacement that inertial navigation location technology captures robot
When positioning.It is substantially the process that a position is constantly updated.The present invention carries out auxiliary inertial navigation using PDR location technology and determines
Position.
2) pathfinding technology in service robot room is studied
The real time position that robot is obtained by RHLED-RSS location technology and PDR location technology, is calculated using pathfinding
The shortest path of method planning robot arrival designated place.The A* algorithm that the present invention uses uses heuristic search, answers indoors
With there is very high efficiency under scene.
The calculating of the avoidance of research indoor service robot provided by the invention is specific as follows:
1) target detection of the robot to obstacle
Infrared obstacle range finder module will be loaded on service robot indoors to detect obstacle, when robot encounters barrier
When hindering, infrared obstacle range finder module to system will feed back and waiting system conveys solution.
2) it plans again in path when encountering obstacle
It is quickly to carry out path to robot to be planned again so as to making that system, which is received the complaint message that robot encounters,
Robot avoids obstacle, and robot is avoided directly to bump against on obstacle, makes its target position of arriving safe and sound.
LED visible light location technology algorithm in the RHLED-RSS algorithm that the present invention uses can be divided into imaging positioning with it is non-
Imaging positioning two major classes, after the factors such as integrated application demonstration required precision, speed, cost consider, triangulation at
For the prefered method of LED visible light of the present invention positioning.
Difference between specific location algorithm shown in following table:
Following table is that LED visible light location algorithm compares
It is as shown in Figure 2: after compared with above-mentioned list, using triangulation need to estimate transmitting terminal and receiving end away from
From, have many methods that ranging may be implemented, as received signal strength (RSS), arrival time (TOA), reaching time-difference (TDOA),
Angle of arrival (AOA) etc..
In triangulation, AOA algorithm is by angle measurement come computer azimuth, precision with higher.However due to
AOA algorithm needs specific angular transducer, and is not easy in conjunction with existing smart machine.TOA and TDOA location algorithm is logical
The measurement to arrival time or time difference is crossed to calculate distance, although no need to send end and receiving end clock are same for TDOA algorithm
Step, but both algorithms are required to realize between each LED of transmitting terminal and be synchronized.Environment area is larger indoors and LED quantity very
It is difficult to realize for synchronizing between each transmitting terminal in the case where more, therefore both methods is all not easy to promote and answer on a large scale
With.RSS establishes the relationship between illumination and distance from the channel model of the downlink of LED, is counted by measurement light intensity
Distance is calculated, has been realized in the positioning accuracy within 10cm at present.In conclusion RSS location algorithm precision is higher and system is simple
List is easily achieved, and is the higher a kind of location algorithm of sexual valence in visible light location algorithm.
MLED-RSS algorithm introduces multiple LED transmitting terminals, effectively increases the precision of indoor positioning, preferably solves screening
Gear problem improves the reliability, stability and applicability of location algorithm.According to more LED thoughts of MLED-RSS, chaos triangle
The Location Theory of shape and the geometric properties of polygon, it is group that the present invention, which is designed and uses a kind of arranged with regular hexagon LED,
New RSS method, referred to as regular hexagon visible light receive signal strength ranging localization algorithm (RHLED-RSS, Regular
Hexagon Light Emitting Diode-Received Signal Strength)。
Using the system of RSS, in order to calculate target robot (R, Robot) coordinate, receiving end at least needs to obtain 3 mutually not
The reference signal of interference, here with the VLC transmission positioning reference information based on white light LEDs.For the letter for guaranteeing adjacent LED sending
It number will not can be caused to interfere between separated and each signal on the receiver of R, (CA), wavelength-division can be distributed using carrier wave
It is multiplexed (WDM), the time division multiplexing technologies such as (TDM) and code division multiplexing (CDM).Assuming that the coordinate of R is located at (xr, yr, zr), and utilize
RSS can obtain R and transmitting terminal TxiDistance be respectively (dc1, dc2, dc3), then positioning equation in three sides is
By solving above-mentioned equation, the position coordinates of R can be obtained.For start coplanar and two-dimensional planar location situation,
That is z1=z2=z3And zr=0.Coordinate (the x of R at this timer, yr) by solving two linear equations.
According to the geometrical property of Chaos Triangle it is found that when progress RSS is positioned, there are one in triangle localization region
It is poor that progress is positioned when a or multiple acute angles, therefore it is best that positioning performance is carried out in the case where LED forms equilateral triangle.Accordingly
The present invention uses 7 LED to arrange for the LED of one group of regular hexagon, as shown in Fig. 2, situation about can be performed in guaranteeing engineering
It ensure that the precision of LED positioning to greatest extent down, and can be carried out using remaining LED when one or more LED are blocked
Obtuse triangle positioning, ensure that the anti-interference of algorithm.
It is as shown in Figure 3: the practical layout situation of RHLED-RSS.Use this topological structure as the arrangement side LED of subelement
Formula can satisfy the visible light location requirement compared with big room.This method can largely improve the accurate of indoor positioning
A possibility that being positioned under degree, stability and complex environment.
As shown in Figure 4: the PDR inertial navigation location technology that the present invention uses, P is in existing indoor inertial navigation location algorithm, PDR
Algorithm is a kind of effective pedestrian's indoor positioning algorithms.The basic principle is that passing through inertia on the basis of determining initial position
Device estimates pedestrian to the measurement of the physical quantity of the acceleration of pedestrian movement, angular speed, suffered gravity and magnetic induction intensity etc.
Relative displacement.The process of PDR algorithm positioning is exactly the process of the continuous renewal of position, and algorithm structure is according to the sensing used
The difference of device type and be slightly different.
The sensor that PDR algorithm is mainly used includes accelerometer, gravitometer, magnetometer and gyroscope in figure, is being needed
It also needs to use barometer when determining height.Due to physical quantity acceleration, the gravity of four kinds of sensor measurements in PDR algorithm
Acceleration, angular speed, magnetic induction intensity are the amounts for having size and Orientation, therefore the measurement data of any time sensor is one
A three-dimensional vector is to indicate that surveyed physical quantity is defining the size and Orientation in coordinate system, herein by the survey of these four sensors
Data are measured to be known as measuring vector.PDR algorithm flow mainly includes that paces detection in figure, cadence estimation, step-size estimation, course are estimated
Meter and 5 modules of dead reckoning.The purpose of paces detection is analysis and the handling machine traveling process by sensing data
In cyclical action, provide pace time point for dead reckoning.Cadence and step-size estimation provide opposite position for dead reckoning
Size is moved, course estimation provides opposite angular displacement size for dead reckoning.Can be seen that from algorithm principle and structure pass through by
Position fixing process goes to step domain from time domain, and position error is at any time when PDR algorithm avoids static caused by inertia device measurement error
Between the problem of increasing.
Machine is carved at the beginning is located at position s0, the coordinate in location coordinate is (x0, y0), it is transported after a step
Move (x1, y1), the distance of movement is l1, direction of motion angle is θ1, then location updating equation be
I represents step number, step-length l in formulaiWith deflection θiIt is the survey using accelerometer, gravitometer, gyroscope and magnetometer
Amount data estimate that therefore, the positioning accuracy of algorithm depends on the precision of sensor measurement and the performance of algorithm for estimating.?
In gait processes, the measurement data of accelerometer and gravitometer can show period and the placement property of walking, therefore general logical
The measurement data of accelerometer and gravitometer is crossed to determine the paces advanced.liGenerally by establishing step-length and measuring physical quantity
Model between statistic is estimated that l is established in existing research mostlyiWith the estimation model of cadence, cadence in such model
Estimated accuracy has large effect to the precision of dead reckoning.θiThe general measurement by gyroscope, magnetometer and gravitometer to
Amount is estimated.It can be used for assisting it since PDR algorithm only provides opposite displacement when application positions indoors
His indoor positioning technologies are to improve the positioning performance of system entirety
The infrared obstacle ranging technology that the present invention uses, wherein infrared detection can classify according to its physical effect are as follows: according to
The luminous effect of certain objects converts photosignal to the photon detection of electric signal；Thermal energy is converted according to the fuel factor of infrared ray
For the thermistor detector of electric signal；Equipment transmitting infrared light passes through warning line or is received the received master of machine by object diffusing reflection
Dynamic infrared acquisition；The infrared signal that the radiation of the sensor collection external world is come in is converted to the passive type infrared acquisition of charge.
As shown in Figure 6: the infrared detection module, fingerprint identification module that the present invention uses belongs to active infra-red detection.Infrared detection module, fingerprint identification module issues
Specific frequency pulsed infrared light receives the reflected light in front using infrared photosensitive receiving tube, hinders according to luminous intensity judgement is received
Hinder object distance.Since its structure is simple, at low cost, it is chiefly used in contactless range measurement, detection of obstacles, the inspection of barrier color
The devices such as survey.
Infrared detection module, fingerprint identification module is powered using button cell, makees master controller using EM78P153 single-chip microcontroller, is sent out every 0.5s
An infrared pulse signal is penetrated, reception pipe receives reflected infrared signal, after amplification filtering, surveys according to pulse width
Measure signal strength, and then disturbance in judgement object distance.When there is transmitting signal to be blocked, module exports TTL low level signal such as Fig. 5
It is shown.
The SCM program design flow diagram of infrared obstacle range finder module.Single-chip microcontroller work crystal oscillator is set in initialization
4MHz opens house dog interruption etc., and the sleep delay of 0.5ms is interrupted by house dog to be completed.Letter is detected twice in succession in 10 μ s
It number can be used to filtering interference signals and effectively control detecting distance if changing detecting distance modify subparameter.Program is set
Continuous 60 times of meter detect signal, i.e., the position of barrier does not change in continuous 30s, it is believed that equipment is placed in closed-loop
Within the border, equipment no longer needs to work at this time, enters continuous sleep state at this time, until external event wakes up, can connect in P62 delivery outlet
Photosensitive sensor, shock sensor etc. are connect for external wake.
It is as shown in Figure 7: on the basis of studying the path planning and navigation algorithm of static environment, by artificial intelligence field A-
Star algorithm is introduced into the solution in static indoor path planning, and improved A-Star algorithm can be advantageously applied to static state
The planning in indoor path can greatly improve arithmetic speed and efficiency of the A-Star algorithm indoors in navigator fix.Static environment
Distinguishing feature be the obstacle information known in environment in advance.Indoor map belongs to typical static environment, its main feature is that respectively
Category feature point type is more, contains much information, and topological relation is complicated.A-Star algorithm based on heuristic search, by indoor ground
Figure is pre-processed, and is optimized to feasible zone, is improved to A-Star algorithm traversal mode, and utilizes heuristic search
Suitable evaluation function is arranged in mode, assesses the position of search, greatly reduces unnecessary search, reduces cost
It can guarantee simultaneously time-consuming less.
A-Star algorithm is to solve shortest path most efficient method in a kind of static road network.Using the side of heuristic search
Formula selects suitable heuristic function, and the cost value of each expanding node is calculated and assessed, to select cost value most
Excellent node is extended, until finding destination node
Need to use evaluation function, general type when the valuation of each node around the evaluation of A-Star algorithm are as follows:
F (n)=g (n)+h (n) (3-3)
In formula, f (n) is evaluation function, is the optimal estimation value that starting point passes through that node n reaches home；G (n) is from initial
The actual consumption of node-to-node n；H (n) is the estimation consumption from node n to target point, embodies the heuristic information of search,
Therefore, h (n) is referred to as enlightening function [8].The heuristic function planned herein using manhatton distance as indoor map, table
Up to formula are as follows:
H (n)=| Xi-Xj|+|Yi-Yj| (3-4)
In order to guarantee that A-Star algorithm can find minimal cost path, the figure scanned for must satisfy following condition:
1. the number of the descendant node of each node in search graph is limited；2. the cost of all arcs is both greater than some in search graph
Positive number ε；3. for all node n in search graph, estimation of the h (n) without departing from actual value h (n).
Need to use two lists: openlist list and closelist list in the working principle of A-Star algorithm.
Openlist list is used to record all nodes for being calculated but not propagating through, is used to record in closelist list
The node propagated through.Every time in search, it is ranked up according to node evaluation function f (n) value size each in openlist list,
The smallest node of f (n) value is deleted, and is added in closelist list, then it is extended, is circuited sequentially, until searching
Rope is to destination node.The flow chart of A-Star algorithm is as shown in Figure 7.
Since A-Star algorithm uses heuristic search, near barrier, search speed can be dramatically speeded up, and
Near broad feasible zone, search speed can be obvious slack-off.Using this feature of A-Star algorithm, feasible zone is carried out
Linearization process, thus acceleration path search efficiency.
The foregoing is only a preferred embodiment of the present invention, but scope of protection of the present invention is not limited thereto,
In the technical scope disclosed by the present invention, any changes or substitutions that can be easily thought of by anyone skilled in the art,
It should be covered by the protection scope of the present invention.Therefore, protection scope of the present invention should be with scope of protection of the claims
Subject to.
Claims (6)
1. the service robot operating path calculation method based on visible light-seeking, it is characterised in that: the path calculation method is adopted
Evading for path planning and barrier is carried out with A-star algorithm；Using RHLED-RSS location algorithm and PDR inertial navigation location algorithm
Dual ranging calculating is carried out for practical ranging or prediction ranging；
The path calculation method uses transmitting terminal of the LED light source as signal, is sent out while providing illumination modulated
Optical signal；Auxiliary inertial navigation positioning is carried out using PDR inertial navigation location algorithm；The PDR inertial navigation location algorithm needle that service robot uses
Optical auxiliary signal carries out action positioning when to relative displacement；
The path computing side obtains the real-time position of service robot by RHLED-RSS location algorithm, PDR inertial navigation location algorithm
It sets, then reaches the shortest path of designated place by A-star algorithmic rule robot；
The service robot built-in infrared obstacle range finder module, infrared obstacle range finder module detect barrier；By red
The detection information of outer obstacle range finder module feedback carries out evading for path planning and barrier by A-star algorithm.
2. the service robot operating path calculation method according to claim 1 based on visible light-seeking, feature exist
In: the A-star algorithm selects suitable heuristic function using heuristic search mode,
It is calculated and is assessed for the cost value of each expanding node, evaluation function is calculated by following formula:
F (n)=g (n)+h (n)
In formula, f (n) is evaluation function, is the optimal estimation value that starting point passes through that node n reaches home；G (n) is from start node
To the actual consumption of node n；H (n) is the estimation consumption from node n to target point；
Evaluation function obtained by calculation is calculate by the following formula out the heuristic function of map planning:
H (n)=| Xi-Xj|+|Yi-Yj|
X in formulai, YiAnd Xj, YjIndicate the two-dimensional coordinate of position i and position j in map.
3. the service robot operating path calculation method according to claim 2 based on visible light-seeking, feature exist
In: the state of openlist list and closelist list records node is used in the A-star algorithm；
Openlist list is used to record to calculate the node not extended by A-star algorithm；
Closelist list is for recording the node extended；
A node evaluation function f (n) obtained by calculation is applied to openlist list, the openlist in A-star algorithm
List is ranked up for each node evaluation function f (n) value size, deletes the smallest node of f (n) value, and be added to
It is extended in closelist list and is circuited sequentially, until searching destination node.
4. the service robot operating path calculation method according to claim 1 based on visible light-seeking, feature exist
Obtain measurement vector from the sensor in service robot in: the PDR inertial navigation location algorithm, the sensor be speedometer,
Gravitometer, magnetometer, gyroscope；
PDR inertial navigation positioning calculation establishes initial time machine and is located at position s0, and establishing coordinate in location coordinate is (x0,
y0)；Service robot shifting then updates coordinate after moving a step be (x1, y1), indoors by following equation calculation service robot
Square coordinate:
L in formula1For the distance of movement, θ is transported1To move deflection, i is step number.
5. the service robot operating path calculation method according to claim 1 based on visible light-seeking, feature exist
In: the coordinate (x of three-dimensional space midpoint r locating for service robot is established in the RHLED-RSS location algorithmr, yr, zr), and
Set transmitting terminal TxiDistance be respectively (dc1, dc2, dc3)；The position coordinates of service robot are obtained by following equation:
R is the point in three-dimensional space in formula.
6. the service robot operating path calculation method according to claim 1 based on visible light-seeking, feature exist
In: the LED light source is 7, and 7 LED light sources are arranged at regular hexagon shape.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910664173.6A CN110231627A (en) | 2019-07-23 | 2019-07-23 | Service robot operating path calculation method based on visible light-seeking |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910664173.6A CN110231627A (en) | 2019-07-23 | 2019-07-23 | Service robot operating path calculation method based on visible light-seeking |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110231627A true CN110231627A (en) | 2019-09-13 |
Family
ID=67855857
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910664173.6A Pending CN110231627A (en) | 2019-07-23 | 2019-07-23 | Service robot operating path calculation method based on visible light-seeking |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110231627A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113686341A (en) * | 2021-08-23 | 2021-11-23 | 中建照明有限公司 | LIFI-based indoor navigation method and system for visually impaired people |
| CN114061578A (en) * | 2020-07-30 | 2022-02-18 | 南宁富桂精密工业有限公司 | Indoor positioning path optimization method, electronic device and computer readable storage medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102749084A (en) * | 2012-07-10 | 2012-10-24 | 南京邮电大学 | Path selecting method oriented to massive traffic information |
| CN106527448A (en) * | 2016-12-16 | 2017-03-22 | 浙江工业大学 | Improved A* robot optimal path planning method suitable for warehouse environment |
| CN108225326A (en) * | 2017-12-31 | 2018-06-29 | 南京理工大学 | A kind of AGV paths planning methods based on A* algorithms |
-
2019
- 2019-07-23 CN CN201910664173.6A patent/CN110231627A/en active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102749084A (en) * | 2012-07-10 | 2012-10-24 | 南京邮电大学 | Path selecting method oriented to massive traffic information |
| CN106527448A (en) * | 2016-12-16 | 2017-03-22 | 浙江工业大学 | Improved A* robot optimal path planning method suitable for warehouse environment |
| CN108225326A (en) * | 2017-12-31 | 2018-06-29 | 南京理工大学 | A kind of AGV paths planning methods based on A* algorithms |
Non-Patent Citations (3)
| Title |
|---|
| 吴楠 等: "基于多LED的高精度室内可见光定位方法", 《电子与信息学报》 * |
| 彭澎: "基于A*算法的路径规划算法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑（月刊）》 * |
| 王春生: "基于动态A*路径规划算法的移动机器人室内定位方法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑（月刊）》 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114061578A (en) * | 2020-07-30 | 2022-02-18 | 南宁富桂精密工业有限公司 | Indoor positioning path optimization method, electronic device and computer readable storage medium |
| CN113686341A (en) * | 2021-08-23 | 2021-11-23 | 中建照明有限公司 | LIFI-based indoor navigation method and system for visually impaired people |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108012326B (en) | The method and chip of robot monitoring pet based on grating map | |
| CN106052676B (en) | A kind of robot navigation's localization method, device and robot | |
| CN102927980B (en) | Indoor positioning system and indoor positioning method based on three-dimensional multipoint wireless and micro-inertia navigation | |
| Al Nuaimi et al. | A survey of indoor positioning systems and algorithms | |
| CN107943051A (en) | Indoor AGV navigation methods and systems based on Quick Response Code guiding with visible light-seeking | |
| CN106405605B (en) | A kind of indoor and outdoor seamless positioning method and positioning system of the robot based on ROS and GPS | |
| Xianjia et al. | Applications of uwb networks and positioning to autonomous robots and industrial systems | |
| Xiao et al. | Comparison and analysis of indoor wireless positioning techniques | |
| CN107076826B (en) | Ultra-broadband ranging method and apparatus, barrier-avoiding method and avoidance equipment | |
| CN102768352B (en) | Underground ultra-wideband electromagnetic and ultrasonic combined positioning system and method based on pattern matching | |
| CN102253367A (en) | Ultrasonic wave based indoor three-dimensional positioning system and method | |
| CN107071899A (en) | Real-time positioning system in a kind of quick high accuracy room | |
| CN101986758A (en) | Method for positioning wireless sensor network | |
| CN102033222A (en) | Large-scale multiple-object ultrasonic tracking and locating system and method | |
| CN109917788A (en) | A kind of control method and device of Robot wall walking | |
| CN106441306A (en) | Intelligent life detecting robot with capabilities of independent positioning and map building | |
| CN103616025A (en) | Three-dimensional field staff positioning navigation system | |
| CN110231627A (en) | Service robot operating path calculation method based on visible light-seeking | |
| CN207010998U (en) | Real-time positioning apparatus in a kind of quick high accuracy room | |
| Zhang et al. | Review on UWB-based and multi-sensor fusion positioning algorithms in indoor environment | |
| CN202772928U (en) | Underground target positioning system based on distance constraint | |
| CN109884589A (en) | A kind of degree of precision visible light Indoor Robot positioning device | |
| Linde | On aspects of indoor localization | |
| Maxim et al. | Trilateration localization for multi-robot teams | |
| CN206133305U (en) | Intelligence optics guidance system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20190913 |