CN109115209A - Personnel positioning method and device in a kind of piping lane - Google Patents

Personnel positioning method and device in a kind of piping lane Download PDF

Info

Publication number
CN109115209A
CN109115209A CN201810802354.6A CN201810802354A CN109115209A CN 109115209 A CN109115209 A CN 109115209A CN 201810802354 A CN201810802354 A CN 201810802354A CN 109115209 A CN109115209 A CN 109115209A
Authority
CN
China
Prior art keywords
coordinate
particle
personnel
bluetooth
bluetooth beacon
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.)
Granted
Application number
CN201810802354.6A
Other languages
Chinese (zh)
Other versions
CN109115209B (en
Inventor
闵涛
罗小勇
杨丛昊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan yunjiangna micro Information Technology Co.,Ltd.
Original Assignee
Hunan Wagner Micro Mdt Infotech Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hunan Wagner Micro Mdt Infotech Ltd filed Critical Hunan Wagner Micro Mdt Infotech Ltd
Priority to CN201810802354.6A priority Critical patent/CN109115209B/en
Publication of CN109115209A publication Critical patent/CN109115209A/en
Application granted granted Critical
Publication of CN109115209B publication Critical patent/CN109115209B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/04—Position of source determined by a plurality of spaced direction-finders

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The present invention provides personnel positioning method and device in a kind of piping lane, piping lane inner wall is provided with N number of Bluetooth beacon, N >=2, personnel positioning method includes: to determine the first coordinate transform formula using coordinate of the coordinate and corresponding personnel of any two Bluetooth beacon in inertial coodinate system in piping lane；Step (2): coordinate of the computing staff in map coordinates system initializes the coordinate and course angle of each particle of particle collection；Step (3): the coordinate and course angle of particle are updated using the step-length of personnel's walking and the angle increment of course angle, calculate the weight of each particle；Step (4): the estimated value of coordinate and Weight the computing staff coordinate in map coordinates system of each particle is utilized.Cost is greatly saved in the sparse arrangement of Bluetooth beacon of the present invention, is easy to implement, strong applicability, in conjunction with inertial navigation, bluetooth and cartographic information, solves orientation problem when personnel's long-time high-precision real in the piping lane of city.

Description

Personnel positioning method and device in a kind of piping lane
Technical field
The present invention relates to personnel positioning method and devices in a kind of localization method and device more particularly to a kind of piping lane.
Background technique
Piping lane positions the scope for belonging to indoor positioning, and the common method of indoor positioning, which specifically includes that, utilizes merely inertial navigation Method, the localization method based on ultra wide band (UWB), the method for RFID and method based on bluetooth etc..Method based on inertial navigation Can there are problems that the accumulation of error and direction drift, be unable to satisfy prolonged location requirement, the method for UWB receives the shadow of multipath It rings seriously, and complexity of constructing, can not be accurately positioned especially within the scope of non line of sight, the method for RFID or bluetooth exists again Positioning accuracy is poor, needs the disadvantages of dense arrangement.
With the development of the modern information technologies such as wireless network, mobile computing, location based service is widely used to Various aspects in masses' production, life.And the successive foundation of GPS, GLONASS and BEI-DOU position system, so that in real time, accurately Realization outdoor positioning it is no longer difficult.But due to the factors such as signal blocks, multiple tracks effect influence, indoors when, the satellites such as GPS Positioning system is unable to satisfy requirement of the people to positioning accuracy.Therefore, various indoor positioning technologies, method are come into being.These In indoor orientation method, have and be based purely on a certain technology, such as: based on bluetooth, being based on WIFI, based on ultra wide band and based on micro- Inertial navigation etc. also has and combines several technologies, overcomes respective disadvantage, the method for realizing positioning, such as: by micro- inertial navigation and ultra wide band Fusion, micro- inertial navigation are merged with WIFI.
Pipe gallery is the comprehensive corridor of pipeline for building Urban Underground in, wherein be equipped with electric power, communication, combustion gas, heat supply, The various utilities pipelines such as plumbing, localizing environment are complicated.In this environment, the localization method certainty based on radio technology because It is blocked for signal, the factors such as multiple tracks effect, so that positioning cost increases, accuracy decline.Determined based on radio signal strength Position method (such as bluetooth positioning, WIFI positioning, radio frequency ID positioning), positioning accuracy is poor and needs dense arrangement.And ultra-wide Band positioning, since piping lane is long and narrow and turnout is more, can not play the wide advantage of ultra wide band orientation range, need to increase base station deployment, Lead to increase cost and difficulty of construction, and equally can due to block and the influence of multiple tracks effect and position accuracy decline.And it is based on The method of inertial navigation due to not depending on external information when work, therefore is not easily susceptible to the interference of complicated piping lane environment, but set due to used It is standby to be carried convenient for personnel, therefore inertial navigation can only use micro- inertial navigation technique, and micro- inertial navigation based on micro-electro-mechanical sensors, error Accumulation and direction drift are all bigger, are unable to satisfy prolonged location requirement.
When the simple progress personnel positioning using IMU, since the accumulation of error of inertial reference calculation and direction drift about, position error meeting What is become with accumulated time is unacceptable.
Summary of the invention
The present invention is in order to solve the affected by environment big and location technology based on inertial navigation of the location technology based on radio The larger problem of error accumulation proposes personnel positioning method and device in a kind of piping lane.
In order to solve the above technical problems, the technical solution adopted by the present invention is that: a kind of personnel positioning method in piping lane, it is special Sign is: the top or bottom of piping lane inner wall are provided with N number of Bluetooth beacon, N >=2, the bluetooth number of each Bluetooth beacon and its Coordinate in map coordinates system is it is known that personnel positioning method includes: in the piping lane
Step (1): believed using coordinate of any two Bluetooth beacon in map coordinates system and personnel from the two bluetooths Coordinate of the personnel that measurement obtains when passing through immediately below mark in inertial coodinate system determines the first coordinate transform formula, and described first sits Mark the coordinate transform formula that transform is coordinate of the coordinate into map coordinates system in inertial coodinate system；
Step (2): building particle collection utilizes coordinate and the first coordinate transform formula meter of the personnel in inertial coodinate system Coordinate of the calculation personnel in map coordinates system utilizes coordinate pair of the personnel in map coordinates system in adjacent two step of personnel's walking The coordinate and course angle of each particle of particle collection are initialized；
Step (3): using personnel walking a step step-length and in map coordinates system the step course angle relative to preceding The angle increment of the course angle of one step concentrates the coordinate of each particle and course angle to be updated on particle, calculates the power of each particle Weight；
Step (4): the Weight meter of coordinate of each particle in map coordinates system He the particle is concentrated using particle The estimated value of calculation personnel coordinate in map coordinates system.
In the present invention, at least two Bluetooth beacons are set in the top and bottom of piping lane.When personnel are from successively by two When Bluetooth beacon, since coordinate of the Bluetooth beacon in map coordinates system is it is known that can be by the two Bluetooth beacons on ground The coordinate of coordinate and personnel in inertial coodinate system in figure coordinate system determines that the coordinate of inertial coodinate system and map coordinates system becomes Relationship is changed, consequently facilitating bluetooth coordinate, inertial coordinate, map reference are unified in map coordinates system.Since particle filter is used Cartographic information has been arrived, and has finally been also to be transformed into map reference to be shown on map, so will be first with the first coordinate Transform is by the coordinate transformation in inertial coodinate system to map coordinates system in the following, particle filter again.
Further include following steps (P1-1)-(P1-5) before step (1) in above-mentioned technical proposal:
Step (P1-1): establishing map coordinates system, determines the bluetooth number of each Bluetooth beacon and its in map coordinates system In coordinate, and enable designer walk step k=0, enable the first mark align_flag=0；
Step (P1-2): if personnel's row makes a move, k=k+1, and the scanning bluetooth letter in the kth step of personnel's walking are enabled Mark and the signal strength for acquiring Bluetooth beacon, and measure tkCoordinate of the moment personnel in inertial coodinate systemIts In, personnel complete when kth walking is walked as tkMoment；
Step (P1-3): judging whether align_flag=2 is true, if the determination result is YES, thens follow the steps (3), no Then follow the steps (P1-4)；
Step (P1-4): judge whether pass through from the underface of Bluetooth beacon in the kth step of personnel's walking, if judging result Be it is yes, then enable align_flag=align_flag+1, then record coordinate of the Bluetooth beacon in map coordinates system (xP(align_flag),yP(align_flag)), and by tkCoordinate of the moment personnel in inertial coodinate system is denoted as (xQ(align_flag), yQ(align_flag))；
Step (P1-5): judging whether align_flag=2 is true, if the determination result is YES, then first carries out step (1) again Step (P1-2) is jumped to, step (P1-2) is otherwise jumped to；
In the step (1): utilizing the coordinate (x of Bluetooth beaconP(1),yP(1)) and (xP(2),yP(2)) and personnel inertia sit Coordinate (x in mark systemQ(1),yQ(1)) and (xQ(2),yQ(2)) determine that the first coordinate transform formula, the first coordinate transform formula are
Wherein, coordinate (x (k), y (k)) is tkCoordinate of the moment personnel in map coordinates system, Δ θ=tan-1((yP(2)- yP(1))/(xP(2)-xP(1)))-tan-1((yQ(2)-yQ(1))/(xQ(2)-xQ(1)))；
The step (2) are as follows: seat of each particle in map coordinates system is concentrated to particle using coordinate (x (k), y (k)) Mark is initialized, and coordinate (x is utilizedP(2),yP(2)) and coordinate (x (k), y (k)) concentrate each particle in map reference on particle Course angle in system is initialized, and coordinate (x (k), y (k)) is according to tkCoordinate of the moment personnel in inertial coodinate system (xIMU(k),yIMU(k)) and the first coordinate transform formula be calculated；
The step (3) are as follows: using the first course angle increment to tkCourse of the moment each particle in map coordinates system Angle is updated, and utilizes the step-length of the kth step of personnel's walking, tkCourse angle of the moment each particle in map coordinates system is to tk Coordinate of the moment each particle in map coordinates system is updated, and calculates tkThe weight w of moment each particlei(tk), described Course of the course angle for the kth step that one course angle increment is walked for personnel in map coordinates system relative to the course angle of -1 step of kth Angle increment, first course angle increment is calculated according to coordinate (x (k-1), y (k-1)), coordinate (x (k), y (k)), described The step-length of the kth step of personnel's walking is calculated according to coordinate (x (k-1), y (k-1)), coordinate (x (k), y (k)), the coordinate (x (k-1), y (k-1)) utilizes the first coordinate transform formula and coordinate (xIMU(k-1),yIMU(k-1)) be calculated, the coordinate (x (k), y (k)) according to the first coordinate transform formula and coordinate (xIMU(k),yIMU(k)) be calculated；
The step (4) are as follows: utilize tkMoment particle concentrates coordinate and t of each particle in map coordinates systemkMoment Particle concentrates the weight of each particle, weighted calculation tkThe estimated value of moment personnel coordinate in map coordinates system is returned and is executed Step (2)；
Preferably, in the step (P1-5), further includes: judgement (xP(1),yP(1)) and (xP(2),yP(2)) it whether is same It is a, it if the determination result is YES, then enables align_flag=1 and jumps to step (P1-2), no then follow the steps (1) jumps again To step (P1-2).
In the present invention, if judgementWithFor the same point, then can not be calculated using two Bluetooth beacons Coordinate transform formula, therefore the signal strength of step (P1-2) acquisition Bluetooth beacon need to be jumped to, then judge whether by second Bluetooth beacon.
In above-mentioned technical proposal, in the step (P1-4), judge in the kth step of personnel's walking from n-th of Bluetooth beacon Underface pass through condition beIt sets up, wherein n ∈ { 1,2 ..., N }, rssk,nFor in step (P1-2) The kth of personnel's walking walks the signal strength of interior collected n-th of Bluetooth beacon, whereinFor n-th Bluetooth beacon First calibration value of signal strength；
Preferably, in the step (P1-3), further includes:
Judge whether to meet first condition, the first condition is that there are rssk,n, so thatIt sets up,
If judging, j=2 is set up and judgement is unsatisfactory for first condition, thens follow the steps (3)；
If judging, j=2 is set up and judgement meets first condition, concentrates each particle in map coordinates system on particle Coordinate is reset, and concentrates each particle in map coordinates system using coordinate pair particle of n-th of Bluetooth beacon in map coordinates system In coordinate initialized, and course angle of each particle in map coordinates system is constant, then executes step (3)；
If judging, j=2 is invalid, thens follow the steps (P1-4).
In above-mentioned technical proposal,When repeatedly to pass through below n-th of Bluetooth beacon, each collected bluetooth The minimum value in set that the maximum value of the signal strength of beacon is constituted；
Preferably,It is calculated according to maximum signal scaling method, the maximum signal scaling method packet It includes:
Step (A1): personnel successively pass through from the underface of N number of Bluetooth beacon, and this process is repeated q times, are swept with fixation It retouches intermittent scanning Bluetooth beacon and acquires the signal strength of Bluetooth beacon, and all indigo plants that will can be scanned in a scan period The bluetooth of tooth beacon is numbered and the signal strength of the Bluetooth beacon is stored as a frame data, utilizes each frame data of storage Construct data set G；
Step (A2): from data set G with time sequencing by the signal strength that frame extracts n-th of Bluetooth beacon obtain arranging to AmountWherein, RSSn,mIndicate the signal of n-th of Bluetooth beacon in m frame data Intensity enables RSS if not including the signal strength of n-th of Bluetooth beacon in m frame datan,m=RSSmin；
Step (A3): willIn be less than RSScutValue be set to RSSmin, wherein RSSmin< RSScut；
Step (A4): to obtained in step (A3)Carry out smothing filtering；
Step (A5): obtained in extraction step (A4)All maximum；
Step (A6): using the minimum value in all maximum obtained in step (A5) as the letter of n-th of Bluetooth beacon Number intensity scale definite value
Step (A7): step (A2)-(A6) is repeated, all N number of Bluetooth beacons are obtained
Preferably, RSScut< -60dB.Preferably, RSSmin< -90dB.
Bluetooth received signals when being passed through below the Bluetooth beacon due to personnel are there are difference, and " personnel are from indigo plant for setting There are difficulty for the threshold value of the Bluetooth received signals of the underface process of tooth beacon ".In the present invention, personnel successively believe from N number of bluetooth Pass through immediately below target, and this process is repeated q times.So as to incite somebody to action " when repeatedly by below n-th of Bluetooth beacon, every time The minimum value in set that the maximum value of the signal strength of collected Bluetooth beacon is constituted " is as under n-th of Bluetooth beacon The judgment threshold just passed through.What this method was calculatedWhether value is avoided in practical applications " under Bluetooth beacon Failing to judge, misjudging in the judgement just passed through ", to be effectively utilized the Bluetooth beacon of each laying.This method computational efficiency height, Calculated result is accurate.
In above-mentioned technical proposal, in the step (P1-3), further includes:
Judge whether to meet first condition, the first condition is that there are rssk,n, so thatIt sets up；
If judging, j=2 is set up and judgement is unsatisfactory for first condition, thens follow the steps (3)；
If judging, j=2 is set up and judgement meets first condition, concentrates each particle in map coordinates system on particle Coordinate is reset, and concentrates each particle in map coordinates system using coordinate pair particle of n-th of Bluetooth beacon in map coordinates system In coordinate initialized, and course angle of each particle in map coordinates system is constant, then executes step (3)；
If judging, j=2 is invalid, thens follow the steps (P1-4).
Personnel in the process of walking, may pass through below Bluetooth beacon again, if judgement meets first condition, that is, receive The Bluetooth signal arrived is greater than threshold value, then judges that personnel pass through below Bluetooth beacon.Since Bluetooth beacon is in map coordinates system Coordinate be it is known that then reinitialize the coordinate of particle using the coordinate of the Bluetooth beacon, so as to using accurate The coordinate of Bluetooth beacon reduces the error in positioning.Can repeatedly through Bluetooth beacon laying since personnel walk in piping lane, Therefore the accumulation that can avoid position error, to improve positioning accuracy.
In above-mentioned technical proposal, in the step (3) further include: judge whether to meet second condition, the second condition For there are rssk,n, so thatRss_min_limit is the Bluetooth signal of Bluetooth beacon Intensity Availability Threshold value；
If the determination result is YES, then according to rssk,nCalculate tkThe weight of moment each particle, and according to tkMoment each grain The weight of son carries out resampling to particle, and otherwise the weight of particle remains unchanged；
Preferably, if judgement meets second condition, t is calculated using following formulakThe weight w of i-th of particle of momenti(tk)
Wherein,rangen (tk) it is tkThe distance between moment personnel and n-th of Bluetooth beacon, range_bi,n(tk) it is tkI-th of particle of moment and n-th The distance between a Bluetooth beacon, σ ranget(m) standard deviation for the Gaussian Profile that measurement error is obeyed；
It is highly preferred that calculating t using following formulakThe distance between moment personnel and n-th of Bluetooth beacon
Decay factor when wherein c_en is Bluetooth signal transmission in localizing environment, RSSrangeFor away from being adopted at Bluetooth beacon 1m The signal strength of the Bluetooth beacon collected.
In the present invention, positioning result is weighted using each particle coordinate and weight.Particle weights are available The Bluetooth received signals received calculate.In the present invention, when judgement meets second condition, i.e., Bluetooth received signals are greater than certain threshold Value (personnel are close enough to which Bluetooth beacon is available at a distance from Bluetooth beacon), just utilizes the Bluetooth received signals meter received Particle weights are calculated, the accuracy of weight calculation is which thereby enhanced.
In above-mentioned technical proposal, the signal that the kth that personnel walk in step (P1-2) walks interior collected Bluetooth beacon is strong Degree is the average value of the signal strength of the collected Bluetooth beacon in the kth of personnel's walking walks.
In the present invention, subsequent calculating is carried out using the average value that kth walks the signal strength of interior Bluetooth beacon so that calculating, sentencing Disconnected result is more accurate.
In above-mentioned technical proposal, the binaryzation grating map of piping lane is it is known that in the binaryzation grating map, and personnel are not It can be set as 0 by the gray value of the pixel in region, personnel can be set as 1 by the gray value of the pixel in region, described It include the position of all metope contour lines in binaryzation grating map；
Between the step (3) and step (4) further include:
Step (P3-1): judge personnel walking kth walk in i-th of particle track whether pass through wall surface contour line, institute Stating metope contour line is contour line of the wall by the corridor side of piping lane；
If the determination result is YES, then 0 is set by the weight of i-th of particle or set minimum just for the weight of the particle Value is so that the particle can be replaced by other particles when resampling, i=1,2 ..., I, and to all particle weights wi(tk) returned One changes, and carries out resampling to particle, executes step (4)；
Preferably, if judging, i-th of particle meets third condition, judges interior i-th of the particle of kth step of personnel's walking Track pass through wall surface contour line, the third condition are and the pixel in all closest binaryzation grating maps of the first line segment There are the pixel that pixel is 0 in point, the coordinate of two endpoints of the first line segment is respectively tkMoment, tk-1Moment i-th Coordinate of the son in map coordinates system.
In the present invention, personnel positioning result is corrected using map.By judge particle track whether pass through wall surface wheel Profile determines whether to set particle weights to minimum positive value or 0, to give up particle through walls in resampling.
In above-mentioned technical proposal, the step (P3-1) further include: judge personnel walking kth step in whether all particles Track both pass through metope contour line；
If judging result be it is no, then follow the steps (4)；
If the determination result is YES, then course angle of each particle in map coordinates system is concentrated to retain on particle, and will be each Coordinate of a particle in map coordinates system resets and reinitializes；
Preferably, if judging, the track of all particles in the kth step of personnel's walking both passes through metope contour line, utilizes point QkCoordinate pair particle concentrate the coordinate of each particle to be initialized, point QkCoordinate be calculated according to following steps:
Step (B1): by point Pk-jIn upright projection to the metope contour line, the Q being located on wall contour line is obtainedk-j Point, point Pk-jFor tk-jThe estimated value of position of the moment personnel in map coordinates system, j >=3；Preferably, j >=5；
Step (B2): from point Qk-jStart successively to take a little on the wall contour line towards the locality that particle hits wall Qk-(j-1)、Qk-(j-2)、……、Qk, so that Qk-jWith Qk-(j-1)Between, Qk-(j-1)With Qk-(j-2)Between ..., Qk-1With QkBetween The path length of wall contour line is respectively equal to point Pk-jWith Pk-(j-1)Between, point Pk-(j-1)With Pk-(j-2)Between ..., point Pk-1 With PkThe distance between, point Pk-(j-1)、Pk-(j-2)、……、PkRespectively tk-(j-1)Moment, tk-(j-2)Moment ..., tkMoment people The estimated value of position of the member in map coordinates system；
Step (B3): the first track is translated, and makes invocation point Pdrk-jIt is moved to point Qk-jPosition, first track For point Pdrk-j、Pdrk-(j-1)、……、PdrkThe track of composition, point Pdrk-j、Pdrk-(j-1)、……、PdrkRespectively tk-jWhen It carves, tk-(j-1)Moment ..., tkPosition of the moment personnel in inertial coodinate system；
Step (B4): by the first track after translation with point Qk-jCentered on put rotation, obtain point Pdr 'k-j、Pdr ′k-(j-1)、……、Pdrk' the second track for constituting, and make invocation point Pdr 'k-jTo Pdr 'k-(j-1)Direction and point Pk-jTo Pk-(j-1) Direction it is identical, wherein the point Pdr ' on the second trackk-j、Pdr′k-(j-1)、……、Pdrk' be respectively the first track on point Pk-j、Pk-(j-1)、……、PkPoint where after translation and rotation；
Step (B5): the matching degree d_match of outline is calculated using following formula
Wherein, d (Pdr 'k-j,Qk-j)、d(Pdr′k-(j-1),Qk-(j-1))、……、d(Pdr′k,Qk) it is respectively point Pdr 'k-j And Qk-jBetween, point Pdr 'k-(j-1)And Qk-(j-1)Between ..., point Pdrk' and QkThe distance between；
Step (B6): judging whether d_match >=Thres_match is true, if the determination result is YES, then utilizes point Qk's Coordinate pair particle concentrates coordinate of each particle in map coordinates system to be initialized, otherwise, then by point Qk-jAlong the wall Towards point Q on facial contour linek-(j-1)Moving distance Δ S obtains new point Qk-j, step (B2)-(B5) is repeated, until d_match >=Thres_match is set up, and wherein Thres_match is matching degree threshold value；
It is highly preferred that in the step (B6), if judge d_match >=Thres_match set up and judge d_move >= Thres_move is set up, then concentrates each particle in map on particle using the intersection point of the track of each particle and metope contour line Coordinate in coordinate system is initialized, and wherein d_move is initial point Qk-jTo new point Qk-jMovement total distance, Thres_move is mobile total distance threshold value.Personnel complete when kth-j walking is walked as tk-jMoment.
Personnel in the process of walking, there may be the problem of particle all hits wall when fork in the road is turned round, after will affect Continuous positioning.In the present invention, if judging, particle all hits wall, takes multiple moment forward from this moment, using it is multiple when Position of the quarter personnel in inertial coodinate system reinitializes particle position, so as to quickly correct personnel when particle hits wall Position is convenient for quickly positioning.
In above-mentioned technical proposal, the distance on the beeline channel in piping lane between two Bluetooth beacons is not less than 10m；
Preferably, it is not more than 50m in the distance in beeline channel between two Bluetooth beacons；
Preferably, the corner of piping lane, piping lane are provided with Bluetooth beacon at intersection.
In the present invention, each Bluetooth beacon sparse can be arranged, i.e., in the distance in beeline channel between two Bluetooth beacons Not less than 10m, so that deployment cost has been saved, convenient for quickly laying.The distance between two Bluetooth beacons are not no in beeline channel Greater than 50m, so that personnel can be during traveling by passing through the below or above of Bluetooth beacon, to be existed by Bluetooth beacon Coordinate pair positioning result in map coordinates system is corrected again, is avoided the excessive accumulation of error, is improved positioning accuracy. By being provided with Bluetooth beacon at intersection in the corner of piping lane, piping lane, so that being easy to position when personnel are located at It, can be timely by coordinate of the Bluetooth beacon in map coordinates system when at the corner of the piping lane of deviation, piping lane intersection Positioning result is corrected, accumulated error is avoided, improves positioning accuracy.
The present invention also provides personnel positioning apparatus in a kind of piping lane, it is characterised in that: personnel positioning apparatus in the piping lane It include: inertia measuring module, for coordinate of the survey crew in inertial coodinate system,
Bluetooth receiving module, for scanning the signal strength of Bluetooth beacon and acquisition Bluetooth beacon；
Signal processing unit: for using coordinate of any two Bluetooth beacon in map coordinates system and personnel from this two Coordinate of the personnel that measurement obtains when passing through immediately below a Bluetooth beacon in inertial coodinate system determines the first coordinate transform formula, institute The coordinate transform formula that the first coordinate transform formula is coordinate of the coordinate into map coordinates system in inertial coodinate system is stated, personnel are utilized The coordinate of each particle of coordinate pair particle collection of the personnel in map coordinates system and course angle carry out in adjacent two step of walking Initialization, wherein coordinate of the personnel in map coordinates system utilizes coordinate and first coordinate of the personnel in inertial coodinate system Transform calculate, using personnel walking a step step-length and in map coordinates system the step course angle relative to back The angle increment of course angle concentrates the coordinate of each particle and course angle to be updated on particle, calculates the weight of each particle；Benefit Concentrate the Weight computing staff of coordinate of each particle in map coordinates system and the particle in map coordinates system with particle The estimated value of middle coordinate；
The inertia measuring module and bluetooth receiving module connect with signal processing unit and are fixedly mounted on personnel's foot Portion, waist, an arbitrary position in shoulder.
The advantages and positive effects of the present invention are: due to it is existing using bluetooth arrange scene, mostly need every A Bluetooth beacon is arranged in 5m, therefore cost is greatly saved in the sparse arrangement of the Bluetooth beacon of the application, is easy to implement, and is applicable in Property is strong.The present invention is the characteristics of combining inertial navigation and bluetooth, and comprehensive utilization inertial navigation, bluetooth and cartographic information solve in the piping lane of city Orientation problem when personnel's long-time high-precision real, and the sparse deployment of localizer beacon, no cable are constructed without light current, deployment, maintenance It is easy with migration.The present invention comprehensively utilizes inertial navigation, bluetooth and cartographic information piping lane localization method, low in pipe gallery to realize Cost, high-precision, prolonged positioning.In the present invention, comprehensive utilization inertial navigation, bluetooth and cartographic information are made using particle filter For Data Fusion Filtering device, can in the case where a small amount of, sparse laying Bluetooth beacon, realize in the piping lane of city personnel for a long time, In high precision, it positions in real time.
Detailed description of the invention
In order to more clearly explain the technical solutions in the embodiments of the present application, make required in being described below to embodiment Attached drawing is briefly described, it should be apparent that, the drawings in the following description are only some examples of the present application, for For those of ordinary skill in the art, without any creative labor, it can also be obtained according to these attached drawings His attached drawing.
Fig. 1 is a kind of overall plan schematic diagram of the localization method of embodiment of the embodiment of the present invention.
Fig. 2 is the specific scheme schematic diagram of Fig. 1.
Fig. 3 be inertia measuring module in personnel positioning apparatus in a kind of piping lane of embodiment of the embodiment of the present invention, The connection schematic diagram of bluetooth receiving module and signal processing module.
Fig. 4 is a kind of two point alignment methods based on Bluetooth beacon of embodiment of the embodiment of the present invention.
Fig. 5 is a kind of signal of corner's setting Bluetooth beacon in piping lane of embodiment of the embodiment of the present invention Figure.
Fig. 6 is a kind of setting Bluetooth beacon at the piping lane intersection of piping lane of embodiment of the embodiment of the present invention Schematic diagram.
Fig. 7 is a kind of point Q of embodiment of the embodiment of the present inventionkCoordinate calculating schematic diagram.
Fig. 8 is the actual location effect picture that personnel positioning method obtains in the piping lane using the embodiment of the present invention.
In figure, 11, inertia measuring module, 12, bluetooth receiving module, 2, Bluetooth beacon, 10, signal processing unit, 20, indigo plant Tooth beacon.In figure, black triangle great circle is the bluetooth deployed position for track correct, and triangle is to be aligned for track Bluetooth deployed position, dashed trace be personnel practical track route, realize zone circle track be utilize this localization method estimation Personnel track route.
Specific embodiment
Below in conjunction with the attached drawing of the application, the technical scheme in the embodiment of the application is clearly and completely described, Obviously, described embodiments are only a part of embodiments of the present application, instead of all the embodiments.Based in the application Embodiment, every other embodiment obtained by those of ordinary skill in the art without making creative efforts, all Belong to the range of the application protection.
As Figure 1-Figure 8, the present invention provides personnel positioning method and device in a kind of piping lane.
Piping lane localization method overall plan described in this patent is as shown in Figure 1.Personnel carry positioning label, and positioning label is solid Surely it is placed in personnel's foot, in three positions of waist or shoulder arbitrary position.Positioning label includes inertia measuring module, bluetooth Receiving module.Inertia measuring module is preferably IMU module.Inertia measuring module provides the real-time relative position letter of positioning label Breath, the i.e. two-dimentional rectangular co-ordinate relative to positioning starting point.In the present invention, the position phase of inertia measuring module and bluetooth receiving module Together or with fixed positional relationship.
Localization method, as Data Fusion Filtering device, will position relative position, the utilization that label provides using particle filter Bluetooth signal intensity obtains ranging localization information and localization region map three is merged, and finally obtains positioning label in map In position, to obtain the positioning result of personnel.
In the present invention, the location information and cartographic information of Bluetooth beacon are used as observed quantity in particle filter, play to mark Label relative position information is filtered modified effect.
And pipe gallery is mostly narrow structure, by the way that the track IMU is merged with piping lane map with particle filter method, This narrow structure can drift about to the direction of IMU and position accumulated error plays and inhibits correcting action well.Due to comprehensive benefit With IMU, map and Bluetooth beacon, so that positioning when the localization method can realize personnel's long-time high-precision real in the piping lane of city Problem, and the sparse deployment of localizer beacon, no cable are constructed without light current, and deployment, maintenance and migration are easy.
In the present invention, using two Bluetooth beacons by the track IMU from IMU relative coordinate system (coordinate in inertial coodinate system) Middle alignment (transformation) is into map coordinates system.The track IMU snapped in map coordinates system is modified.It is most important Correction effect show themselves in that when carry positioning label pedestrian work in piping lane when, if there is original place move up and down (step on as follows, Stand up, bend over, original place quick short steps etc.), at this time IMU can generate mistake track (especially positioning label be placed on waist, shoulder when), So that pedestrian is substantially in situ, but the position location IMU shows that people advances toward some direction always, and the amendment of ground constraint diagram can not Effectively correct this positional fault.At this point, the Bluetooth beacon laid in piping lane can correct this position location mistake in time.It is logical It crosses and Bluetooth beacon is set above or below piping lane, position location will be limited near the operating position.When pedestrian leaves work When making position by below Bluetooth beacon, position location will be corrected near the Bluetooth position.
The overall implementation process of this localization method is as shown in Figure 2.Steps are as follows:
Step (P1-1): it is loaded into the Bluetooth beacon deployed position table comprising all N number of Bluetooth beacons.Setting carries positioning mark The personnel of label walk step number as k=0, and setting the first mark align_flag=0, setting utilizes Bluetooth beacon amendment position location Bluetooth signal intensity Availability Threshold value rss_min_limit is arranged in enabler flags beacon_enable=true.Execute (P1- 2)；
Step (P1-2): the bluetooth number and signal strength letter that bluetooth module on positioning label is swept to are received in chronological order Breath waits inertia measuring module (preferably IMU module) on positioning label to generate step position data.If personnel's row makes a move, i.e., When having step generation, k=k+1 seeks the average signal strength values that each Bluetooth beacon is swept in a step respectively.It executes (P1-3).
Multiple bluetooth signal intensity can be swept in a step of personnel's walking, bluetooth signal intensity of the invention is using average Signal strength indication, maximum signal value, it is preferred to use average signal strength values can make positioning result more stable.
Step (P1-3) executes (3) if align_flag=2, into particle filter makeover process.Otherwise, it executes (P1-4), into the track IMU alignment procedure.
Step (P1-4): judge whether pass through from the underface of Bluetooth beacon in the kth step of personnel's walking, if judging result Be it is yes, then enable align_flag=align_flag+1, then record coordinate of the Bluetooth beacon in map coordinates system (xP(align_flag),yP(align_flag)), and by tkCoordinate of the moment personnel in inertial coodinate system is denoted as (xQ(align_flag), yQ(align_flag))；If align_flag=1, second Bluetooth beacon is searched for.Otherwise, first Bluetooth beacon is searched for.Such as The signal strength indication of all Bluetooth beacons searched out between one step of fruit personnel walking is both less than rss_min_limit, then returns Receipt row (P1-2) otherwise searches for bluetooth signal intensity point of maximum intensity, and saves the corresponding bluetooth of current demand signal peak signal value and compile Number and the relative position coordinates that provide of corresponding IMU.If being unsatisfactory for search termination condition, execution (P1-2) is returned, otherwise, Align_flag=1 is set, the relative position IMU that the number of winning the confidence point of maximum intensity goes out is denoted as Q1(xQ(1),yQ(1)), it is corresponding with peak signal value Bluetooth number search Bluetooth position table, obtain actual coordinate P of the personnel in map at this time1(xP(1),yP(1))；Using same Method search for second Bluetooth beacon.Obtain Q2(xQ(2),yQ(2)) and P2(xP(2),yP(2)), the current relative position IMU is carried out Alignment transformation, the position P after obtaining alignment3(xP(3),yP(3)), if P2With P3It is same, then it returns execution (P1-2), it is no Then, align_flag=2 is set.P3For personnel's walking to P1Coordinate of the personnel in map coordinates system when completing to walk in next step afterwards. It is possible that personnel do not move at starting position, then possible P2 and P3 is the same point, can not determine course angle, can not carry out grain Son filtering.
The kth of keeper's walking walks the maximum value pair of the signal strength of the collected Bluetooth beacon for meeting first condition The bluetooth for the Bluetooth beacon answered is numbered, and the coordinate (x of corresponding Bluetooth beacon is numbered according to the bluetoothP(1),yP(1)) obtain tkMoment Coordinate of the personnel in map coordinates system is (xP(1),yP(1)), utilize the inertia measurement data meter of the kth step acquisition of personnel's walking Calculate tkCoordinate (x of the moment personnel in inertial coodinate systemQ(1),yQ(1))。
Step (P1-5): judging whether align_flag=2 is true, if the determination result is YES, then first carries out step (1) again Step (P1-2) is jumped to, step (P1-2) is otherwise jumped to；
Step (1): believed using coordinate of any two Bluetooth beacon in map coordinates system and personnel from the two bluetooths Coordinate of the personnel that measurement obtains when passing through immediately below mark in inertial coodinate system determines the first coordinate transform formula, and described first sits Mark the coordinate transform formula that transform is coordinate of the coordinate into map coordinates system in inertial coodinate system.
The relative position that label is obtained by inertia device is either positioned, or positioning label passes through Bluetooth beacon and obtains Ranging information or cartographic information be relative to respective relative coordinate system, to carry out positioning fusion it may first have to It is coordinately transformed, by three's coordinate transform into the same coordinate system.In this localization method, map coordinates system is chosen as benchmark Coordinate system, and the relative position coordinates for positioning label and Bluetooth beacon position coordinates are transformed into map coordinates system.Wherein, When Bluetooth beacon position coordinates are by deployment Bluetooth beacon, the coordinate of corresponding position acquisition, therefore bluetooth are marked in map Beacon coordinate has been the coordinate in map coordinates system, therefore is not required to do it and does coordinate transform again.So signified in the present invention Coordinate alignment, refer to and transform to the relative position coordinates for positioning label in map coordinates system.
In the present invention, it is aligned using the track IMU.By in positioning scene installation position coordinate in target-based coordinate system , two Bluetooth beacons separated by a distance, then personnel carry the positioning mark for being equipped with IMU module and bluetooth receiving module Label are successively passed by from the proximal most position away from two bluetooths, corresponding when searching by being respectively received maximum signal when two bluetooths The method of IMU tracing point is realized in the target-based coordinate system where IMU relative coordinate track quickly to be snapped to Bluetooth beacon.
In this localization method, it is the two o'clock pair based on Bluetooth beacon that coordinate alignment, which is carried out using two Bluetooth beacons, Qi Fa, as shown in Figure 4.In Fig. 4, bluetooth P1, P2 is in map coordinates system O-xy, and IMU positioning track is in IMU opposing right angles coordinate It is in O '-x ' y '.Assuming that the floor projection of two Bluetooth beacons is respectively P1(xP(1),yP(1)) and P2(xP(2),yP(2)), personnel take The floor projection value for the relative coordinate that label provides when tape label is gone to immediately below two Bluetooth beacons is respectively Q1(xQ(1), yQ(1)) and Q2(xQ(2),yQ(2)), then, utilize vector P2P1With vector Q2Q1Between relativeness, calculate to obtain planar IMU Track is aligned transformation matrix of coordinates such as following formula:
Δ θ=tan-1((yP(2)-yP(1))/(xP(2)-xP(1)))-tan-1((yQ(2)-yQ(1))/(xQ(2)-xQ(1)))
Wherein, (xIMU(k),yIMU(k)) be IMU initial trace point coordinate, (x (k), y (k)) be aligned after IMU rail Mark point coordinate, that is, coordinate of the personnel in map coordinates system after being aligned.
Subsequent all inertial positioning results are all converted using the first coordinate transform formula, by inertial coodinate system Coordinate obtain the corresponding coordinate in map coordinates system, then in map coordinates system calculate kth step step delta li,k, boat To angle increment Δ headingi,k.Step-length is identical in different coordinate systems, but course angle increment changes.
Step (2): vector is utilizedInitialize particle filter particle collection Particles { xi,yi,headingi,wi}I, Wherein:
Wherein, init_heading=atan2 (yP(3)-yP(2),xP(3)-xP(2)), i ∈ { 1,2 ..., I }, I are particle collection The total number of middle particle, wiFor the weight of i-th of particle, pos_noiseiAnd head_noiseiRespectively the first white noise sequence With the second white noise sequence, the first white noise sequence is the particle position white noise of initialization, and the second white noise sequence is initial The course angle white noise of change.(xQ(3),yQ(3)) it is that personnel pass through Bluetooth beacon P2In inertial coordinate when walking is completed in next step later Coordinate in system.Initial heading angle init_heading in order to obtain, by Bluetooth beacon P2(xP(2),yP(2)) after need to go further One step, by adjacent Q2、Q3Obtain inceptive direction angle.
Step (3): it carries out one step of population and propagates, the position in map coordinates system is snapped to more using label relative position Propagation model of the new model as particle filter, model form such as following formula:
Wherein, (xi(k),yiIt (k)) is tkCoordinate of i-th of the particle of moment in map coordinates system, headingiIt (k) is tk Direction of advance of i-th of the particle of moment in map coordinates system, Δ l (k) are the personnel being calculated according to inertia measurement data The step-length of the kth step of walking, Δ heading (k) are the direction of advance of the kth step of personnel's walking in map coordinates system relative to the The course angle increment of the direction of advance of k-1 step, i ∈ { 1,2 ..., I }, I are the total number that particle concentrates particle.
In particle filter, the particle of particle collection is by obtaining to suggestion distribution (or proposal distribution) sampling, at this In algorithm, it is proposed that distribution on above-mentioned propagation model by adding the Gaussian noise that mean value is zero to obtain.
Step (4): weighted sum is sought by particle collection particle state and particle weights, calculates the estimation of current persons position Value utilizes tkMoment particle concentrates coordinate and t of each particle in map coordinates systemkMoment particle concentrates each particle Weight, weighted calculation tkThe estimated value of moment personnel coordinate in map coordinates system.
In the present invention, on the beeline channel in piping lane, one Bluetooth beacon can be set every 50m, it is if desired higher Precision can be arranged a Bluetooth beacon every 10m, 20m, 30m, can also be separated by 1m, 2m, 5m and a Bluetooth beacon be arranged.If More high position precision is needed, more Bluetooth beacons can also be set according to actual needs.According to actual cost or positioning accuracy Demand can determine the quantity that Bluetooth beacon need to be set, it will be understood by those skilled in the art that.
In the present invention, piping lane fork in the road, need high accuracy positioning region in lay a small amount of Bluetooth beacons, with amendment The estimated location of particle filter can further improve the stability and accuracy of positioning result.In order to improve when personnel turn round Positioning accuracy is provided with Bluetooth beacon at the corner of piping lane, piping lane intersection, as shown in Figure 5, Figure 6.Straight line away from It is not more than 50m in principle from the distance between upper two Bluetooth beacons, and in the piping lane middle section of the corner of piping lane, intersection A Bluetooth beacon, turning shown in following Fig. 5, Fig. 6, shadow region (its in three-way intersection is arranged in intermediate region at crossing Its type cross crossing is similar with the three-way intersection setting region of Bluetooth beacon).
Inertia measuring module is for acquiring inertia measurement data.Bluetooth signal receiving module is for receiving Bluetooth beacon sending Bluetooth signal.
One scan period of bluetooth, that is, looking for the wifi signal of surrounding, each time scan period etc..? In one step of personnel's walking, there may be one or more scan periods, determines according to actual conditions, mutual not shadow of each scan period It rings, operation can be executed while scanning.
Side wirelessly or non-wirelessly can be passed through between inertia measuring module 11, bluetooth receiving module 12 and signal processing unit 10 Formula transmits data.
The present invention provides a kind of Bluetooth beacon field calibration method.Good required all bluetooths letter is laid at positioning scene Mark, then personnel carry positioning label and successively pass by from the minimum distance of all Bluetooth labels, enroll bluetooth signal intensity number According to, and the process come again above.Later, using Bluetooth beacon demarcation signal intensity extracting method, each indigo plant is obtained Signal strength calibration value at the nearest reach distance of tooth beacon.Bluetooth beacon is as a kind of communication to be worked using radio wave Equipment, indoors under environment, when multiple tracks effect that signal reaches, penetrating object due to decaying etc., signal around beacon Intensity is very unstable, and affected by environment obvious, has seriously affected and has been based in Bluetooth beacon signal strength space with range attenuation Distance measuring method.To be compared accurate ranging localization using Bluetooth beacon, need to carry out one for specific laying environment Beacon strengths calibration to one.
In this localization method, calibration is carried out using signal strength mark at maximum signal calibration and one meter to beacon strengths Determine the method that two ways combines.
Maximum signal calibration is that the signal strength for single Bluetooth beacon in specific installation position is demarcated, and method is Lay good all Bluetooth beacons in positioning scene according to location requirement first, then personnel carry positioning label successively from all Pass through immediately below the beacon for needing to demarcate, acquires bluetooth signal intensity data, and this process is controlled in triplicate.Data are adopted After having collected, with beacon demarcation signal intensity extracting method, extract each beacon to be calibrated be repeated several times by when it is each Minimum value in secondary maximum signal, using the minimum value as the maximum signal calibration value RSS of the beaconmax.Wherein Collected signal strength data saves format are as follows: is stored with time sequencing by frame, every frame data include bluetooth on positioning label The number and its corresponding signal strength indication, data set for all Bluetooth beacons swept within a scan period are denoted as:
G{IDn×m,RSSn×m}N×M, n ∈ 1,2 ..., N }, m=1,2 ..., M
Wherein IDn×mFor the corresponding number of n-th of Bluetooth beacon, RSS in m frame datan×mTo be scanned in m frame data To the signal strength of n-th of Bluetooth beacon, N is that Bluetooth beacon quantity is disposed in piping lane, and M is the total frame number of collected data.
Steps are as follows for beacon demarcation signal intensity extracting method:
(A2) signal strength data for extracting the Bluetooth beacon that number is n by frame from data set G, if in a certain frame not The Bluetooth beacon intensity data for being n comprising number, then it is RSS that the corresponding intensity value of the frame, which is arranged,min(generally take RSSmin< -90, Think that bluetooth signal intensity is unavailable at this time), the data column vector extractedIt indicates；
(A3) RSS is usedcut(RSSmin< RSScut< -60dB) it is rightIntercepting process is carried out, i.e., willIn be less than RSScutValue be set to RSSmin.Later, rightCarry out smothing filtering., so thatSignal at each wave crest near it is continuous (i.e. without being equal to RSSminValue)；Smothing filtering is to avoid jumping.It is right in this stepSmothing filtering is carried out, so thatIn all data constitute waveform it is continuous near each crest value.
(A4) wave crest maximum value is extracted.It is successively searched for since m=1It is greater than RSS when encountering firstminValue When, start to search for maximum signal value, it is next equal to RSS until encounteringminUntil value, the peak signal obtained at this time is strong Angle value, the maximum value of as first wave crest.In this approach, the maximum value of all wave crests is searched out backward；
(A5) minimum value in all wave crest maximum values is taken, as n-th Bluetooth beaconValue.It returns and executes the (1) step extracts the RSS of other beaconsmaxValue.
Signal strength calibration is the signal strength calibration for all beacons at one meter, and validity passes through Bluetooth beacon The consistency of energy is guaranteed.Method is, in free space (using outdoor spacious environment come approximate) measurement Bluetooth beacon in spy When determining transmission power, the signal strength of one meter of distant place, and using the signal strength indication as signal at one meter of all Bluetooth beacons Intensity scale definite value RSSrange。
Signal strength calibration value is in free space, at a distance from the Bluetooth beacon of particular transmission power at described one meter For the signal strength at one meter.
Embodiment 1: the reception signal correction particle weights of Bluetooth beacon are utilized
In the present invention, the reception signal that can use Bluetooth beacon is modified the weight of each particle.
In the step (P1-3), further includes:
Judge whether to meet first condition, the first condition is that there are rssk,n, so thatIt sets up,
If judging, j=2 is set up and judgement is unsatisfactory for first condition, thens follow the steps (3)；
If judging, j=2 is set up and judgement meets first condition, concentrates each particle in map coordinates system on particle Coordinate is reset, and concentrates each particle in map coordinates system using coordinate pair particle of n-th of Bluetooth beacon in map coordinates system In coordinate initialized, and course angle of each particle in map coordinates system is constant, then executes step (3)；
If judging, j=2 is invalid, thens follow the steps (P1-4).
In the step (3) further include: judge whether to meet second condition, the second condition is that there are rssk,n, so thatRss_min_limit is the bluetooth signal intensity Availability Threshold value of Bluetooth beacon； Rss_min_limit is calculated using beacon demarcation signal intensity extracting method in embodiment 2.Preferably, rssk,nFor personnel The average signal strength of n-th of Bluetooth beacon of the kth step acquisition of traveling.
Otherwise, if the average signal strength of the beacon searched out between a step is without rss_min_limit is greater than, otherwise, such as Fruit average signal strength is greater than rss_min_limit and is less than the RSS of the beaconmaxValue then updates particle weights using formula (4), Resampling is carried out to particle collection later, otherwise, the average signal strength measured is more than or equal to the RSS of the beaconmaxIt is worth, then basis 4.2.4 bluetooth ranging described in corrects the second way, reinitializes particle using the floor projection position (x, y) of the beacon Collection, then execute (7)；
Rss_min_limit is obtained according to the attenuation model of Bluetooth beacon, that is, is less than the signal strength, then Bluetooth beacon is not It is credible.For the minimum value in wave crest maximum value, if more than this value, then judgement is located at immediately below bluetooth.rss_min_ Limit is the bluetooth signal intensity Availability Threshold value being rule of thumb arranged, and is used to survey less than the signal strength indication of the threshold value When away from positioning, position error will be very big, become unacceptable, i.e., the bluetooth signal intensity that ought be measured is less than rss_min_ When limit, ranging empirical equation rangei10 × n of=- (× lg (rssi)+RSSrange) failure.Therefore to guarantee positioning accuracy, only The distance for positioning label to bluetooth is calculated with the signal strength indication for being greater than the thresholding.And the value range of rss_min_limit with Bluetooth beacon signal transmission power and bluetooth deployment environment it is related, in this method the value range of the threshold value be -65dB~- 72dB。
If the signal strength for receiving Bluetooth beacon is greater thanIt then forces personnel's coordinate modification to be the Bluetooth beacon Coordinate, and using the coordinate of the Bluetooth beacon as the initial value x of particle filteri(0)、yi(0)、headingi(0), it opens again Beginning particle filter calculates.
In the present invention, weight amendment be divided into use the ranging of Bluetooth beacon signal strength as filtering observation when weight amendment with Weight when land used constraint diagram is as filtering observation corrects two kinds of situations.
T is corrected using following formulakThe weight w of i-th of particle of momenti(tk)
Wherein,rangen (tk) it is tkThe distance between moment bluetooth receiving module and n-th of Bluetooth beacon.
range_bi,n(tk) it is tkThe distance between i-th of particle of moment and n-th of Bluetooth beacon.σ is bluetooth ranging mistake The standard deviation for the Gaussian Profile that difference is obeyed.Bluetooth range error is tkMoment bluetooth receiving module and n-th Bluetooth beacon it Between range measurement error, bluetooth ranging distance is rangen(tk)。rangen(tk) calculation method be: using formula (3) Particle coordinate (x is calculatedt, yt), since the coordinate of n-th of Bluetooth beacon is it is known that therefore distance of the particle to Bluetooth beacon It can be calculated.Wherein wt(k) weight of i-th of particle when completing for kth step, ρ is normalization coefficient, rangenIt (k) is the The distance for opposite n-th of Bluetooth beacon that k step measures when completing, range_bi,n(k) i-th of particle is to the when completing for kth step The distance of n Bluetooth beacon, I are the total number that particle concentrates particle.Rule of thumb, the value range of σ is [0.5,2], For non-normalized weight.range_bi,n(k) calculation method is: particle coordinate (x is calculated using formula (3)i,yi), by It is it is known that therefore the distance of particle to Bluetooth beacon can be calculated in the coordinate of n-th of Bluetooth beacon.
When the signal strength that label receives some Bluetooth beacon is greater than or equal to the RSS of the Bluetooth beaconmaxIt is worth and particle When the distance for filtering the current label position that provides to the bluetooth is greater than the threshold value set, the direct position by label in map It is adapted to floor projection position of the Bluetooth beacon in map.Mode is, in particle filter, utilizes the floor projection of the beacon Position (x, y) reinitializes two states of x and y that particle concentrates each particle, and the state of course angle remains unchanged.
Using Bluetooth beacon carry out indoor positioning, using in bluetooth signal intensity space propagate with range attenuation characteristic, According to the empirical equation of distance and signal strength, the general distance away from bluetooth can be obtained.
In this localization method, the label measured just with bluetooth signal intensity ranging is corrected to the distance of Bluetooth beacon Position of the label in map.
It can be using the distance of label that Bluetooth beacon ranging measures to Bluetooth beacon as the observed quantity of particle filter.At this In mode, the empirical equation for bluetooth ranging is as follows:
Wherein rssiFor the signal strength for n-th of Bluetooth beacon that label currently measures, rangenFor label to n-th of indigo plant The distance (three-dimensional distance) of tooth beacon, decay factor when c_en is Bluetooth signal transmission in localizing environment, RSSrangeFor away from hair Penetrate the received signal strength at node 1m.In particle filter, in conjunction with (3) and (4) formula, amendment can be played to Filtering position and is made With.
C_en is related with bluetooth laying environment, with reference to bluetooth RSSI ranging pertinent literature and combines practical ranging environment, i.e., It can determine c_en and RSSrangeValue range.It is preferred that the value range of c_en is 2~4.
As shown in figure 8,20 be Bluetooth beacon for the positioning result that personnel positioning method in piping lane of the present invention obtains.Fig. 8 In, circle is the position disposed for the Bluetooth beacon of track correct, and triangle is the Bluetooth beacon deployment being aligned for track Position.Dashed trace is the practical route walked of personnel, and diamond shape solid line is the positioning result that benefit is obtained by the present invention. As shown in Figure 8, the present invention realizes preferable locating effect.
Embodiment 2: particle weights are corrected using map
The present embodiment 2 and the difference of embodiment 1 are that two bluetooths then can be only arranged in the occasion not high to required precision Beacon can forbid bluetooth to correct weight, only use map after the first coordinate transform formula is calculated using two Bluetooth beacons Particle weights are modified.
Step (P1-1): it is loaded into localizing environment binaryzation grating map.
The occasion not high to required precision, then can only be arranged two Bluetooth beacons, and bluetooth is then forbidden to correct weight later, It is only corrected with map, then two Bluetooth beacons is only set.Therefore beacon_enable=false is enabled.
Modification method is constrained according to the map, updates particle weights, resampling is carried out to particle collection later, is executed (8)；
In the present invention, settable flag bit beacon_enable does not judge if beacon_enable=false rssk,nWith rss_min_limit rssk,nRelationship, and do not judge withRelationship, in step (3) to particle concentrate After the coordinate and course angle of each particle are updated, only step (P1-3-1) is executed.
Indoors in positioning, position location is limited using not accessibility regions some in map, it can be to positioning As a result effective correcting action is played.And under particle filter frame, it is again relatively easy to do map limitation, therefore localization method exists Cartographic information is incorporated in positioning, to improve locating effect.
Map in this localization method for doing map match is two-value map, i.e., map gray value is only with regard to 0 and 1 two kind Value, 0 value indicate for wall or other objects that not may pass through at the pixel, are blank space at the pixel that gray scale is 1, or can quilt The position penetrated.The core of map match is detection through walls, and method here is to use straight line in two-value map to single particle The Last status position and current location for connecting the particle judge the gray value of all nearest neighbor pixels points of the line segment (every time All to the anchor point line of two time of each particle, judge through walls), if there is the pixel for 0, then show that the particle exists Through walls or other barriers in the propagation of this step.If certain particle is through walls, the weight of the particle is set to a very small positive value ε (generally take 1e-20).After particle filtering resampling, the weight of particle through walls is set as 0 or by the duplication grain of particle not through walls Son replacement.So as to improve particle collection to the approximate accuracy of physical tags position, the amendment to position location is realized.Arest neighbors is For the meaning of nearest neighbours, it will be appreciated by those skilled in the art that.
Particle filter can screen out the direction of mistake, mainly handle mileage error problem herein.Pixel in map It is labeled as 0,1.In practice, particle filter particle concentrates all particles phenomenon through walls all occur this is because label is in map There is biggish error in middle position, and this phenomenon is known as particle below and hits wall.In this case, in order to correcting in time Error is guaranteed filtering accuracy, has been carried out using piping lane wall outline method for processing in localization method.This method principle It is that search particle hits a certain range of wall outline position near wall displacement, and calculates the wall extended according to direction of travel Then the part metope profile is taken forward the history IMU in a period of time with from when the moment by the extending direction put on facial contour Positioning track carries out shape matching, makes rough estimate to current location.Later, near the position of rough estimate, current filter is used Course heading afterwards does further matching with the metope extending direction extended according to direction of travel, improves position estimation accuracy. Then with the course heading (boat behind position and particle filter after outline after the estimated location and current filter To) reinitialize particle filter.
If through walls, the mileage loss in deflection error or personnel's traveling may occur there are two reason.The present invention In, if there is deflection error, it may be that particle is through walls, then give up particle through walls.In the present invention, if there is mileage damage It loses, i.e., the distance that personnel calculated advance is less than the distance really advanced.In practice, if particle hits wall, first according to the map The ambient conditions that middle personnel advance judge whether there is turn；If giving up the particle for hitting wall without turn；If there is turn, compare The track of inertia measurement and the track of corner wall, compensate the loss of mileage, enable positioning track more acurrate.
In the present invention, search particle hits a certain range of wall outline position near wall displacement, and discovery, which exists, turns round；It is right It is right than " the part metope profile " and " taking the history IMU positioning track in a period of time forward from when the moment " (shape matches) Make rough estimate in current location.Near the position of rough estimate, with the course heading and metope extending direction after current filter Further matching is done, that is, calculates mileage loss D and improves position estimation accuracy by the mileage compensating for loss and damage into positioning result. Then with the course (course behind position and particle filter after outline) after the estimated location and current filter again Initialize particle filter.Search particle hit a certain range of wall outline position near wall displacement, and calculate according to Then the part metope profile is taken forward one section with from when the moment by the extending direction put on the metope profile that direction of travel extends History IMU positioning track (track that the coordinate i.e. in inertial coodinate system is constituted) in time carries out shape matching.
Seat of the personnel that the inertia measurement data acquired using inertia measuring module are calculated in inertial coodinate system The piping lane metope track that mark, the track of formation and the binaryzation grating map of piping lane are formed carries out outline.Land used constraint diagram When, particle weights through walls are set to the minimum positive value close to zero, and particle not through walls keeps original weight constant, it is then right All weights are normalized.
Wall outline method when occurring hitting wall to particle by taking j=5 as an example is illustrated.
In Fig. 7, W1, W2, W3 are respectively the wall in corridor, and w1, w2, w3 are respectively the profile that wall leans on corridor side Line.With particle filter treated recently a few step tracks be Pk-5Pk-4Pk-3Pk-2Pk-1PkFrom Pk-1To PkWhen, there is particle and hits Wall, at this point, guaranteeing filtering accuracy in order to correct error in time, using piping lane wall outline method in localization method It has carried out for processing.Take the history IMU positioning track Pdr in nearest a period of timek-5Pdrk-4Pdrk-3Pdrk-2Pdrk-1Pdrk Shape matching is carried out, wherein Pdrk-5、Pdrk-4、Pdrk-3、Pdrk-2、Pdrk-1、PdrkIt is corresponded respectively in Pk-5、Pk-4、Pk-3、 Pk-2、Pk-1、PkStep location point.
Realize step are as follows: by Pk-5On the wall that upright projection hits wall to generation particle, obtain being located on wall contour line Qk-5Point, along Qk-5Place wall contour line hits the locality of wall toward particle, successively takes Qk-4、Qk-3、Qk-2、Qk-1、QkPoint, institute A little all on wall contour line, and guarantee Qk-5To Qk-4Point is equal to P along the path length of wall contour linek-5To Pk-4Distance dk-5,k-4(step-length that kth -5 arrives k-4 step), there is s according to thisn-4,n-3Equal to ln-4,n-3, until sn-1,nEqual to ln-1,n, point Qk-5、 Qk-4、Qk-3、Qk-2、Qk-1、QkThe track of composition is the selected local wall profile for outline.
By the track IMU Pdrk-5、Pdrk-4、Pdrk-3、Pdrk-2、Pdrk-1、PdrkWith local configuration Qk-5、Qk-4、Qk-3、Qk-2、 Qk-1、QkDo outline.Matching process is to translate the entire track IMU first, so that Pdrk-5Point is moved to Qk-5Position, so Afterwards with the Pdr after displacementk-5The track IMU centered on point after the entire translation of point rotation, the track local I MU after being aligned Pdr′k-5、Pdr′k-4、Pdr′k-3、Pdr′k-2、Pdr′k-1、Pdrk' until translation after Pdr 'k-5To Pdr 'k-4Direction and Pk-5 To Pk-4Direction it is identical, calculate translate point on the postrotational track IMU to corresponding local configuration Q laterk-5、Qk-4、Qk-3、 Qk-2、Qk-1、QkOn point distance (Pdr 'k-5To Qk-5Distance, Pdr 'k-4To Qk-4Distance, and so on), and calculate Average distance, using the inverse of the average distance as the matching degree of this outline.
Along Qk-5To Qk-4The mobile Q in directionk-5The distance, delta S of one section of very little of point, obtains new Qk-5Point, then according to institute in (2) The method of stating obtains new Qk-4、Qk-3、Qk-2、Qk-1、Qk, to obtain one section of new metope local configuration.It will by (3) the method The track Pdr that the coordinate of new local configuration and inertial navigation is constitutedk-5、Pdrk-4、Pdrk-3、Pdrk-2、Pdrk-1、PdrkAgain Outline is done, new matching degree is obtained.Δ S is the arc length on wall contour line.
Always according to the above method, mobile Qk-5Point, does outline, until the matching degree being calculated is greater than the threshold of setting It is worth, at this time successful match, by Q at this timekThe nearest non-wall point of position reinitializes position as particle collection.Otherwise, If Qk-5The mobile total distance of point is greater than the mobile total distance threshold value of setting, then it fails to match, then will be nearest at particle point through walls Non- wall point, reinitializes position as particle collection.Piping lane wall outline process terminates.
Δ S value range is about 0.05 to 0.2m.The historical track of described nearest a period of time generally takes nearest 30 step To 10 step tracks.Matching degree threshold value value range is 2~10.Mobile 5~10m of total distance threshold value value range；This method is same Matching suitable for the metope that profile is curve.
Embodiment 3: particle weights are corrected using map
The present embodiment 3 and the difference of embodiment 1 are, in combination with the reception signal and map of Bluetooth beacon, while to particle Weight be modified, i.e., by the method for embodiment 1 and embodiment 2 be used in combination.In step (3), if judgement meets second Condition, then according to rssk,nCalculate tkThe weight of moment each particle；After executing the step (3), executes step (P1), utilize ground Figure is modified the weight of particle, i.e., by judging the weight of particle amendment particle whether through walls, if all particles are through walls, The coordinate of each particle is reinitialized using step (B1)-(B5).
It should be noted that all the embodiments in this specification are described in a progressive manner, each embodiment weight Point explanation is the difference from other embodiments, and the same or similar parts between the embodiments can be referred to each other.
The embodiments of the present invention have been described in detail above, but content is only the preferred embodiment of the present invention, It should not be considered as limiting the scope of the invention.All changes and improvements made in accordance with the scope of the present invention, should all It still belongs within this patent covering scope.After the present invention has been read, those skilled in the art are to of the invention various etc. The modification of valence form each falls within the application range as defined in the appended claims.In the absence of conflict, in the present invention Feature in embodiment and embodiment can be combined with each other.

Claims (10)

1. a kind of personnel positioning method in piping lane, it is characterised in that: the top or bottom of piping lane inner wall are provided with N number of bluetooth letter Mark, N >=2, the bluetooth number of each Bluetooth beacon and its coordinate in map coordinates system are it is known that personnel are fixed in the piping lane Position method include:
Step (1): using coordinate of any two Bluetooth beacon in map coordinates system and personnel from the two Bluetooth beacons just Coordinate of the personnel that measurement obtains when lower section is passed through in inertial coodinate system determines the first coordinate transform formula, and first coordinate becomes Change the coordinate transform formula that formula is coordinate of the coordinate into map coordinates system in inertial coodinate system；
Step (2): building particle collection calculates people using coordinate of the personnel in inertial coodinate system and the first coordinate transform formula Coordinate of the member in map coordinates system utilizes coordinate pair particle of the personnel in map coordinates system in adjacent two step of personnel's walking The coordinate and course angle of each particle of collection are initialized；
Step (3): using personnel walking a step step-length and in map coordinates system the step course angle relative to back The angle increment of course angle concentrate the coordinate of each particle and course angle to be updated on particle, calculate the weight of each particle；
Step (4): the Weight of coordinate of each particle in map coordinates system and the particle is concentrated to calculate people using particle The estimated value of member's coordinate in map coordinates system.
2. personnel positioning method in piping lane according to claim 1, it is characterised in that: further include as follows before step (1) Step (P1-1)-(P1-5):
Step (P1-1): establishing map coordinates system, determines the bluetooth number of each Bluetooth beacon and its in map coordinates system Coordinate, and the person of making us walking step k=0, enable the first mark align_flag=0；
Step (P1-2): if personnel's row makes a move, enabling k=k+1, and in the kth step of personnel's walking scanning Bluetooth beacon and The signal strength of Bluetooth beacon is acquired, and measures tkCoordinate (x of the moment personnel in inertial coodinate systemIMU(k),yIMU(k)), In, personnel complete when kth walking is walked as tkMoment；
Step (P1-3): judging whether align_flag=2 is true, if the determination result is YES, thens follow the steps (3), otherwise holds Row step (P1-4)；
Step (P1-4): judge whether pass through from the underface of Bluetooth beacon in the kth step of personnel's walking, if judging result is It is then to enable align_flag=align_flag+1, then records coordinate of the Bluetooth beacon in map coordinates system (xP(align_flag),yP(align_flag)), and by tkCoordinate of the moment personnel in inertial coodinate system is denoted as (xQ(align_flag), yQ(align_flag))；
Step (P1-5): judging whether align_flag=2 is true, if the determination result is YES, then first carries out step (1) and jumps again To step (P1-2), step (P1-2) is otherwise jumped to；
In the step (1), the coordinate (x of Bluetooth beacon is utilizedP(1),yP(1)) and (xP(2),yP(2)) and personnel in inertial coodinate system In coordinate (xQ(1),yQ(1)) and (xQ(2),yQ(2)) determine that the first coordinate transform formula, the first coordinate transform formula are
Wherein, coordinate (x (k), y (k)) is tkCoordinate of the moment personnel in map coordinates system, Δ θ=tan-1((yP(2)- yP(1))/(xP(2)-xP(1)))-tan-1((yQ(2)-yQ(1))/(xQ(2)-xQ(1)))；
The step (2) are as follows: using coordinate (x (k), y (k)) to particle concentrate coordinate of each particle in map coordinates system into Row initialization, utilizes coordinate (xP(2),yP(2)) and coordinate (x (k), y (k)) concentrate each particle in map coordinates system on particle Course angle initialized, coordinate (x (k), y (k)) is according to tkCoordinate (x of the moment personnel in inertial coodinate systemIMU(k), yIMU(k)) and the first coordinate transform formula be calculated；
The step (3) are as follows: using the first course angle increment to tkCourse angle of the moment each particle in map coordinates system carries out It updates, utilizes the step-length of the kth step of personnel's walking, tkCourse angle of the moment each particle in map coordinates system is to tkMoment is each Coordinate of a particle in map coordinates system is updated, and calculates tkThe weight w of moment each particlei(tk), first course Course angle increment of the course angle for the kth step that angle increment is walked for personnel in map coordinates system relative to the course angle of -1 step of kth, First course angle increment is calculated according to coordinate (x (k-1), y (k-1)), coordinate (x (k), y (k)), personnel's row The step-length for the kth step walked is calculated according to coordinate (x (k-1), y (k-1)), coordinate (x (k), y (k))；
The step (4) are as follows: utilize tkMoment particle concentrates coordinate and t of each particle in map coordinates systemkMoment particle Concentrate the weight of each particle, weighted calculation tkThe estimated value of moment personnel coordinate in map coordinates system, returns to step (2)；
Preferably, in the step (P1-5), further includes: judgement (xP(1),yP(1)) and (xP(2),yP(2)) it whether is the same point, If the determination result is YES, then it enables align_flag=1 and jumps to step (P1-2), no then follow the steps (1) jumps to step again Suddenly (P1-2).
3. personnel positioning method in piping lane according to claim 2, it is characterised in that: in the step (P1-3), also wrap Include: judging whether to meet first condition, the first condition be in the kth step of personnel's walking from any one Bluetooth beacon just Lower section is passed through；
If judging, j=2 is set up and judgement is unsatisfactory for first condition, thens follow the steps (3)；
If judging, j=2 is set up and judgement meets first condition, and particle is concentrated coordinate of each particle in map coordinates system It resets, concentrates each particle in map coordinates system using coordinate pair particle of n-th of Bluetooth beacon in map coordinates system Coordinate is initialized, and course angle of each particle in map coordinates system is constant, then executes step (3)；
If judging, j=2 is invalid, thens follow the steps (P1-4).
4. personnel positioning method in piping lane according to claim 2, it is characterised in that: in the step (3) further include: sentence It is disconnected whether to meet second condition, the second condition be do not pass through from the underface of Bluetooth beacon in the kth step of personnel's walking and There are rssk,n, so that rssk,n> rss_min_limit, wherein rss_min_limit is the bluetooth signal intensity of Bluetooth beacon Availability Threshold value；
If the determination result is YES, then according to rssk,nCalculate tkThe weight of moment each particle, and according to tkMoment each particle Weight carries out resampling to particle, and otherwise the weight of particle remains unchanged；
Preferably, if judgement meets second condition, t is calculated using following formulakThe weight w of i-th of particle of momenti(tk)
Wherein,rangen (tk) it is tkThe distance between moment personnel and n-th of Bluetooth beacon, range_bi,n(tk) it is tkI-th of particle of moment and n-th The distance between a Bluetooth beacon, σ rangen(tk) the standard deviation of Gaussian Profile obeyed of measurement error；
It is highly preferred that calculating t using following formulakThe distance between moment personnel and n-th of Bluetooth beacon
Decay factor when wherein c_en is Bluetooth signal transmission in localizing environment, RSSrangeFor away from being collected at Bluetooth beacon 1m The Bluetooth beacon signal strength.
5. personnel positioning method in piping lane described in -4 according to claim 1, it is characterised in that: judge the kth step of personnel's walking The condition that the interior underface from n-th Bluetooth beacon is passed through isIt sets up, wherein n ∈ { 1,2 ..., N }, rssk,nThe signal strength of interior collected n-th of Bluetooth beacon is walked for the kth of personnel's walking, whereinFor n-th of bluetooth First calibration value of the signal strength of beacon；
Preferably,When repeatedly to pass through below n-th of Bluetooth beacon, the signal of each collected Bluetooth beacon is strong The minimum value in set that the maximum value of degree is constituted；
It is highly preferred thatIt is calculated according to maximum signal scaling method, the maximum signal scaling method includes:
Step (A1): personnel successively pass through from the underface of N number of Bluetooth beacon, and this process is repeated q times, with scanning constant week Phase scans Bluetooth beacon and acquires the signal strength of Bluetooth beacon, and all bluetooths that can be scanned in a scan period are believed Target bluetooth number and the signal strength of the Bluetooth beacon are stored as a frame data, are constructed using each frame data of storage Data set G；
Step (A2): column vector is obtained by the signal strength that frame extracts n-th of Bluetooth beacon with time sequencing from data set GWherein, RSSn,mIndicate that the signal of n-th of Bluetooth beacon in m frame data is strong Degree enables RSS if not including the signal strength of n-th of Bluetooth beacon in m frame datan,m=RSSmin；
Step (A3): willIn be less than RSScutValue be set to RSSmin, wherein RSSmin< RSScut；
Step (A4): to obtained in step (A3)Carry out smothing filtering；
Step (A5): obtained in extraction step (A4)All maximum；
Step (A6): the minimum value in all maximum obtained in step (A5) is strong as the signal of n-th of Bluetooth beacon Calibrations
Step (A7): step (A2)-(A6) is repeated, all N number of Bluetooth beacons are obtained
6. personnel positioning method in the piping lane according to any one of claim 2-4, it is characterised in that: the of personnel's walking It is strong for the signal of the collected Bluetooth beacon in the kth step of personnel's walking that k walks the signal strength of interior collected Bluetooth beacon The average value of degree.
7. personnel positioning method in piping lane described in any one of -4 according to claim 1, it is characterised in that: the binaryzation of piping lane Grating map is it is known that in the binaryzation grating map, and personnel can not be set as 0 by the gray value of the pixel in region, Personnel can be set as 1 by the gray value of the pixel in region, include all metope contour lines in the binaryzation grating map Position；It further include step (P3-1) between the step (3) and step (4),
Step (P3-1): judge personnel walking kth walk in i-th of particle track whether pass through wall surface contour line, the wall Facial contour line is contour line of the wall by the corridor side of piping lane,
If the determination result is YES, then it sets the weight of i-th of particle to 0 or sets minimum positive value for the weight of the particle to make The particle can be replaced by other particles when obtaining resampling, i=1,2 ..., I, and to all particle weights wi(tk) carry out normalizing Change, resampling is carried out to particle, is executed step (4)；
Preferably, if judging, i-th of particle meets third condition, judges that the kth of personnel's walking walks the track of interior i-th of particle Pass through wall surface contour line, the third condition be in the pixel in all closest binaryzation grating maps of the first line segment There are the pixels that pixel is 0, and the coordinate of two endpoints of the first line segment is respectively tkMoment, tk-1Moment, i-th of particle existed Coordinate in map coordinates system.
8. personnel positioning method in piping lane according to claim 7, it is characterised in that: the step (P3-1) further include: Judge whether the track of all particles both passes through metope contour line in the kth step of personnel's walking,
If judging result be it is no, then follow the steps (4),
If the determination result is YES, then course angle of each particle in map coordinates system is concentrated to retain on particle, and by each grain Coordinate of the son in map coordinates system resets and reinitializes；
Preferably, if judging, the track of all particles in the kth step of personnel's walking both passes through metope contour line, utilizes point Qk's Coordinate pair particle concentrates the coordinate of each particle to be initialized, point QkCoordinate be calculated according to following steps:
Step (B1): by point Pk-jIn upright projection to the metope contour line, the Q being located on wall contour line is obtainedk-jPoint, point Pk-jPosition when kth-j walking is walked in map coordinates system is completed for personnel；
Step (B2): from point Qk-jStart successively to take a little on the wall contour line towards the locality that particle hits wall Qk-(j-1)、Qk-(j-2)、……、Qk, so that Qk-jWith Qk-(j-1)Between, Qk-(j-1)With Qk-(j-2)Between ..., Qk-1With QkBetween The path length of wall contour line is respectively equal to point Pk-jWith Pk-(j-1)Between, point Pk-(j-1)With Pk-(j-2)Between ..., point Pk-1 With PkThe distance between, point Pk-(j-1)、Pk-(j-2)、……、PkRespectively tk-(j-1)Moment, tk-(j-2)Moment ..., tkMoment people The estimated value of position of the member in map coordinates system；
Step (B3): the first track is translated, and makes invocation point Pdrk-jIt is moved to point Qk-jPosition, first track be point Pdrk-j、Pdrk-(j-1)、……、PdrkThe track of composition, point Pdrk-j、Pdrk-(j-1)、……、PdrkRespectively tk-jMoment, tk-(j-1)Moment ..., tkPosition of the moment personnel in inertial coodinate system；
Step (B4): by the first track after translation with point Qk-jCentered on put rotation, obtain point Pdr 'k-j、Pdr ′k-(j-1)、……、Pdr′kThe second track constituted, and make invocation point Pdr 'k-jTo Pdr 'k-(j-1)Direction and point Pk-jTo Pk-(j-1) Direction it is identical, wherein the point Pdr ' on the second trackk-j、Pdr′k-(j-1)、……、Pdr′kPoint on respectively the first track Pk-j、Pk-(j-1)、……、PkPoint where after translation and rotation；
Step (B5): the matching degree d_match of outline is calculated using following formula
Wherein, d (Pdr 'k-j,Qk-j)、d(Pdr′k-(j-1),Qk-(j-1))、……、d(Pdr′k,Qk) it is respectively point Pdr 'k-jAnd Qk-j Between, point Pdr 'k-(j-1)And Qk-(j-1)Between ..., point Pdr 'kAnd QkThe distance between；
Step (B6): judging whether d_match >=Thres_match is true, if the determination result is YES, then utilizes point QkCoordinate Coordinate of each particle in map coordinates system is concentrated to initialize on particle, otherwise, then by point Qk-jAlong the metope wheel Towards point Q on profilek-(j-1)Moving distance Δ S obtains new point Qk-j, repeat step (B2)-(B5), until d_match >= Thres_match is set up, and wherein Thres_match is matching degree threshold value；
It is highly preferred that in the step (B6), if judge d_match >=Thres_match set up and judge d_move >= Thres_move is set up, then concentrates each particle in map on particle using the intersection point of the track of each particle and metope contour line Coordinate in coordinate system is initialized, and wherein d_move is initial point Qk-jTo new point Qk-jMovement total distance, Thres_move is mobile total distance threshold value.
9. personnel positioning method in piping lane described in any one of -4 according to claim 1, it is characterised in that: straight in piping lane The distance between two Bluetooth beacons are not less than 10m in line passage；
Preferably, it is not more than 50m in the distance in beeline channel between two Bluetooth beacons；
Preferably, the corner of piping lane, piping lane are provided with Bluetooth beacon at intersection.
10. personnel positioning apparatus in a kind of piping lane, it is characterised in that: personnel positioning apparatus includes: in the piping lane
Inertia measuring module, for coordinate of the survey crew in inertial coodinate system,
Bluetooth receiving module, for scanning the signal strength of Bluetooth beacon and acquisition Bluetooth beacon；
Signal processing unit: for utilizing coordinate and personnel of any two Bluetooth beacon in map coordinates system blue from the two Coordinate of the personnel that measurement obtains when passing through immediately below tooth beacon in inertial coodinate system determines the first coordinate transform formula, described the One coordinate transform formula is the coordinate transform formula of coordinate of the coordinate into map coordinates system in inertial coodinate system, is walked using personnel Adjacent two step in coordinate pair particle collection of the personnel in map coordinates system each particle coordinate and course angle carry out it is initial Change, wherein coordinate of the personnel in map coordinates system utilizes coordinate and first coordinate transform of the personnel in inertial coodinate system Formula calculates, course of the course angle relative to back of the step using the step-length of a step of personnel's walking and in map coordinates system The angle increment at angle concentrates the coordinate of each particle and course angle to be updated on particle, calculates the weight of each particle；Utilize grain The Weight computing staff of coordinate of each particle in map coordinates system and the particle sits in map coordinates system in subset Target estimated value；
The inertia measuring module and bluetooth receiving module connect with signal processing unit and are fixedly mounted on personnel's foot, waist An arbitrary position in portion, shoulder.
CN201810802354.6A 2018-07-20 2018-07-20 Method and device for positioning personnel in pipe gallery Active CN109115209B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810802354.6A CN109115209B (en) 2018-07-20 2018-07-20 Method and device for positioning personnel in pipe gallery

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810802354.6A CN109115209B (en) 2018-07-20 2018-07-20 Method and device for positioning personnel in pipe gallery

Publications (2)

Publication Number Publication Date
CN109115209A true CN109115209A (en) 2019-01-01
CN109115209B CN109115209B (en) 2022-03-11

Family

ID=64863063

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810802354.6A Active CN109115209B (en) 2018-07-20 2018-07-20 Method and device for positioning personnel in pipe gallery

Country Status (1)

Country Link
CN (1) CN109115209B (en)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109708647A (en) * 2019-03-08 2019-05-03 哈尔滨工程大学 A kind of indoor topological map pedestrian localization method based on fusion feature element
CN109951830A (en) * 2019-02-01 2019-06-28 湖南格纳微信息科技有限公司 A kind of indoor and outdoor seamless positioning method of multi-information fusion
CN110286355A (en) * 2019-06-12 2019-09-27 三峡大学 A kind of underground gallery plan position information measurement method and device
CN110307848A (en) * 2019-07-04 2019-10-08 南京大学 A kind of Mobile Robotics Navigation method
CN111641921A (en) * 2020-05-22 2020-09-08 深圳市三旺通信股份有限公司 UWB-based pipe gallery positioning method, terminal equipment and storage medium
CN112146660A (en) * 2020-09-25 2020-12-29 电子科技大学 Indoor map positioning method based on dynamic word vector
CN112379329A (en) * 2020-11-06 2021-02-19 通号通信信息集团有限公司 Accurate positioning method and system for underground pipe gallery moving object
CN113271540A (en) * 2020-12-29 2021-08-17 浙江中控技术股份有限公司 Bluetooth signal positioning method, multi-signal fusion positioning method and system
CN113701754A (en) * 2021-09-06 2021-11-26 中国矿业大学(北京) Underground three-dimensional accurate positioning system
CN114910081A (en) * 2022-05-26 2022-08-16 阿波罗智联(北京)科技有限公司 Vehicle positioning method and device and electronic equipment
CN115002657A (en) * 2022-05-23 2022-09-02 浙江大学医学院附属第一医院 Medicine monitoring method and system based on multi-dimensional information acquisition and intelligent processing
CN117793892A (en) * 2024-02-28 2024-03-29 南方电网调峰调频发电有限公司 Underground engineering object positioning method and device based on position determination model

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2570772A1 (en) * 2011-09-16 2013-03-20 Deutsches Zentrum für Luft- und Raumfahrt e.V. Method for localisation and mapping of pedestrians or robots using wireless access points
CN103528587A (en) * 2013-10-15 2014-01-22 西北工业大学 Autonomous integrated navigation system
CN105588566A (en) * 2016-01-08 2016-05-18 重庆邮电大学 Indoor positioning system and method based on Bluetooth and MEMS (Micro-Electro-Mechanical Systems) fusion
CN105737826A (en) * 2016-02-24 2016-07-06 中国地质大学（武汉） Indoor positioning method for pedestrian
US20170127242A1 (en) * 2015-11-04 2017-05-04 Tata Consultancy Services Limited Systems and methods for fusing inertial and bluetooth low energy sensor data for localization
CN107289941A (en) * 2017-06-14 2017-10-24 湖南格纳微信息科技有限公司 A kind of indoor orientation method and device based on inertial navigation
CN108151747A (en) * 2017-12-27 2018-06-12 浙江大学 A kind of indoor locating system and localization method merged using acoustical signal with inertial navigation

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2570772A1 (en) * 2011-09-16 2013-03-20 Deutsches Zentrum für Luft- und Raumfahrt e.V. Method for localisation and mapping of pedestrians or robots using wireless access points
US20150031390A1 (en) * 2011-09-16 2015-01-29 Deutsches Zentrum Fur Luft-Und Raumfahrt E.V. Method for localisation and mapping of pedestrians or robots using wireless access points
CN103528587A (en) * 2013-10-15 2014-01-22 西北工业大学 Autonomous integrated navigation system
US20170127242A1 (en) * 2015-11-04 2017-05-04 Tata Consultancy Services Limited Systems and methods for fusing inertial and bluetooth low energy sensor data for localization
CN105588566A (en) * 2016-01-08 2016-05-18 重庆邮电大学 Indoor positioning system and method based on Bluetooth and MEMS (Micro-Electro-Mechanical Systems) fusion
CN105737826A (en) * 2016-02-24 2016-07-06 中国地质大学（武汉） Indoor positioning method for pedestrian
CN107289941A (en) * 2017-06-14 2017-10-24 湖南格纳微信息科技有限公司 A kind of indoor orientation method and device based on inertial navigation
CN108151747A (en) * 2017-12-27 2018-06-12 浙江大学 A kind of indoor locating system and localization method merged using acoustical signal with inertial navigation

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
PEKKA PELTOLA 等: "A tripartite filter design for seamless pedestrian navigation using recursive 2-means clustering and Tukey update", 《2018 IEEE/ION POSITION, LOCATION AND NAVIGATION SYMPOSIUM (PLANS)》 *
TIAN Y 等: "Application of RFID Technology in Urban Underground Comprehensive Pipe Gallery[", 《IOP CONFERENCE SERIES: EARTH AND ENVIRONMENTAL SCIENCE》 *
杨秋侠 等: "城市综合管廊线性定位优化", 《土木工程学报》 *
王铨: "基于MEMS惯性传感器的井下人员定位***", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *
蒲书祥: "基于粒子滤波的个人导航***算法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 *

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109951830A (en) * 2019-02-01 2019-06-28 湖南格纳微信息科技有限公司 A kind of indoor and outdoor seamless positioning method of multi-information fusion
CN109951830B (en) * 2019-02-01 2023-05-02 湖南云箭格纳微信息科技有限公司 Indoor and outdoor seamless positioning method based on multi-information fusion
CN109708647A (en) * 2019-03-08 2019-05-03 哈尔滨工程大学 A kind of indoor topological map pedestrian localization method based on fusion feature element
CN110286355B (en) * 2019-06-12 2023-03-24 三峡大学 Underground gallery plane position information measuring method and device
CN110286355A (en) * 2019-06-12 2019-09-27 三峡大学 A kind of underground gallery plan position information measurement method and device
CN110307848A (en) * 2019-07-04 2019-10-08 南京大学 A kind of Mobile Robotics Navigation method
CN111641921A (en) * 2020-05-22 2020-09-08 深圳市三旺通信股份有限公司 UWB-based pipe gallery positioning method, terminal equipment and storage medium
CN112146660A (en) * 2020-09-25 2020-12-29 电子科技大学 Indoor map positioning method based on dynamic word vector
CN112379329A (en) * 2020-11-06 2021-02-19 通号通信信息集团有限公司 Accurate positioning method and system for underground pipe gallery moving object
CN112379329B (en) * 2020-11-06 2024-04-23 通号通信信息集团有限公司 Accurate positioning method and system for underground pipe gallery moving object
CN113271540A (en) * 2020-12-29 2021-08-17 浙江中控技术股份有限公司 Bluetooth signal positioning method, multi-signal fusion positioning method and system
CN113271540B (en) * 2020-12-29 2023-04-11 浙江中控技术股份有限公司 Bluetooth signal positioning method, multi-signal fusion positioning method and system
CN113701754B (en) * 2021-09-06 2023-05-12 中国矿业大学(北京) Underground three-dimensional accurate positioning system
CN113701754A (en) * 2021-09-06 2021-11-26 中国矿业大学(北京) Underground three-dimensional accurate positioning system
CN115002657A (en) * 2022-05-23 2022-09-02 浙江大学医学院附属第一医院 Medicine monitoring method and system based on multi-dimensional information acquisition and intelligent processing
CN115002657B (en) * 2022-05-23 2024-05-21 浙江大学医学院附属第一医院 Medicine monitoring method and system based on multidimensional information acquisition and intelligent processing
CN114910081B (en) * 2022-05-26 2023-03-10 阿波罗智联(北京)科技有限公司 Vehicle positioning method and device and electronic equipment
CN114910081A (en) * 2022-05-26 2022-08-16 阿波罗智联(北京)科技有限公司 Vehicle positioning method and device and electronic equipment
CN117793892A (en) * 2024-02-28 2024-03-29 南方电网调峰调频发电有限公司 Underground engineering object positioning method and device based on position determination model
CN117793892B (en) * 2024-02-28 2024-05-28 南方电网调峰调频发电有限公司 Underground engineering object positioning method and device based on position determination model

Also Published As

Publication number Publication date
CN109115209B (en) 2022-03-11

Similar Documents

Publication Publication Date Title
CN109115209A (en) Personnel positioning method and device in a kind of piping lane
CN106840178B (en) A kind of map building based on ArcGIS and intelligent vehicle autonomous navigation method and system
CN109951830B (en) Indoor and outdoor seamless positioning method based on multi-information fusion
CN105547305B (en) A kind of pose calculation method based on wireless location and laser map match
EP3136128B1 (en) Trajectory matching using peripheral signal
KR101954677B1 (en) Integrated localization method and apparatus of high accuracy robust to route change
CN102116625B (en) GIS (geographic information system)-GPS (global position system) navigation method of inspection robot
CN103363965B (en) Precision measurement method of underline engineering during station yard transformation
CN103068043B (en) A kind of indoor accurate positioning method based on WIFI and acceleration transducer
CN106710293A (en) Dynamic and intelligent vehicle guidance method for underground parking lot
CN206704211U (en) Heavy haul train alignment system based on movable block
CN102323582B (en) Autonomous orbit determination method for satellite based on synthetic aperture radar
CN107360549A (en) A kind of indoor positioning air navigation aid based on bluetooth 5
CN106093857A (en) A kind of mobile anchor node WSN assisted location method based on helix
CN104796866A (en) Indoor positioning method and device
KR20180087827A (en) SLAM method and apparatus robust to wireless environment change
CN104931990A (en) Golf course navigation map surveying and mapping method
CN103760585B (en) Satellite and earth combination positioning method applicable to forest region
CN109031379A (en) A kind of generation method and system of navigation routine map
CN106525044A (en) Person positioning navigation system and method of large naval ship based on ship structure chart
CN113923596A (en) Indoor positioning method, device, equipment and medium
Li et al. Automatic construction of radio maps by crowdsourcing PDR traces for indoor positioning
CN104864858A (en) Laying method and device of engineering control network
CN108156580A (en) A kind of indoor floor location method based on smart mobile phone
CN104931987A (en) Golf terminal equipment course navigation method

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
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address

Address after: 410205 1705, building 1b, Hunan military civilian integration science and Technology Innovation Industrial Park, No. 699, Qingshan Road, Changsha high tech Development Zone, Changsha, Hunan

Patentee after: Hunan yunjiangna micro Information Technology Co.,Ltd.

Address before: 410205 No. 5, 4th Floor, Building F4, Yuyuan, Lugu, 27 Wenxuan Road, Changsha High-tech Development Zone, Hunan Province

Patentee before: HUNAN GLONAVIN INFORMATION TECHNOLOGY CO.,LTD.

CP03 Change of name, title or address