CN112099039A - Indoor relative positioning method based on laser radar - Google Patents
Indoor relative positioning method based on laser radar Download PDFInfo
- Publication number
- CN112099039A CN112099039A CN202010876496.4A CN202010876496A CN112099039A CN 112099039 A CN112099039 A CN 112099039A CN 202010876496 A CN202010876496 A CN 202010876496A CN 112099039 A CN112099039 A CN 112099039A
- Authority
- CN
- China
- Prior art keywords
- laser radar
- indoor
- point cloud
- cloud data
- points
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 238000005070 sampling Methods 0.000 claims abstract description 52
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 43
- 238000012545 processing Methods 0.000 claims abstract description 13
- 238000010606 normalization Methods 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 2
- 238000002474 experimental method Methods 0.000 abstract description 5
- 230000003068 static effect Effects 0.000 abstract description 3
- 238000012795 verification Methods 0.000 abstract description 3
- 230000035945 sensitivity Effects 0.000 abstract description 2
- 238000005259 measurement Methods 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000000007 visual effect Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012821 model calculation Methods 0.000 description 2
- 244000291564 Allium cepa Species 0.000 description 1
- 238000012952 Resampling Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000012612 static experiment Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Physics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Computational Mathematics (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Optimization (AREA)
- Databases & Information Systems (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- Computer Hardware Design (AREA)
- Software Systems (AREA)
- Automation & Control Theory (AREA)
- Operations Research (AREA)
- Algebra (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The invention relates to an indoor relative positioning method based on a laser radar, which applies a random sampling consistency algorithm to indoor relative positioning. The method comprises the steps of improving a classical random sampling consistency algorithm, processing indoor multi-line laser radar point cloud data by utilizing the improved random sampling consistency algorithm, fitting the multi-line laser radar point cloud data to obtain a wall surface equation of an indoor environment, and solving the position of a radar relative to the indoor environment. Through a ground static verification experiment, the positioning precision of the indoor relative positioning method based on random sampling consistency can reach 2cm, and the expected index requirement (0.1m) is met. The method solves the problem of strong interference of indoor positioning, can deal with indoor complex environment, has small sensitivity to indoor environment change, can improve positioning efficiency by reasonably selecting parameters, and is suitable for being applied to indoor positioning engineering of unmanned aerial vehicles.
Description
Technical Field
The invention belongs to an indoor relative positioning method, relates to an indoor relative positioning method based on a laser radar, and relates to a method for realizing indoor relative positioning of an unmanned aerial vehicle by using a multi-line laser radar.
Background
In recent years, the unmanned aerial vehicle technology is rapidly developed, the application field is more and more wide, and the flight space is gradually expanded to the indoor, and is applied to the fields of indoor shooting, environment modeling, indoor reconnaissance and the like. The indoor relative positioning of the unmanned aerial vehicle is a very important link of the unmanned aerial vehicle technology. Indoor relative positioning, namely under indoor environment, obtains the position information of the unmanned aerial vehicle relative to the indoor environment.
Although the navigation system based on GPS positioning is widely used for positioning the unmanned aerial vehicle, in an indoor environment, GPS signals are easily interfered and shielded, and thus cannot be used. Therefore, how to realize indoor relative positioning of the unmanned aerial vehicle is a challenging problem. At present, mainly adopt multiple type sensor measurement such as acoustic signal, electromagnetic signal, visual information and Inertia Measurement Unit (IMU) measurement to realize unmanned aerial vehicle indoor relative positioning, wherein, mainly through sonar sensor, through the distance information between ultrasonic measurement and the target based on the acoustic signal. Indoor relative positioning based on electromagnetic signals requires acquiring magnetic field data of detailed signals with sufficient density and uploading the data to a server, and position information is obtained through comparison of the magnetic field signals. Both the acoustic wave signal and the electromagnetic signal are easy to interfere and have low precision. The indoor relative positioning based on the visual information mainly collects visual signals through a camera, and the indoor positioning is realized through image matching, feature extraction and other modes. The positioning based on the inertial measurement unit mainly solves the data of the gyroscope and the accelerometer through a digital platform so as to obtain position information, and the biggest problem is that errors are accumulated and cannot be used independently. In the literature, "Somani a, Chung S, Celik k. mono-vision core SLAM for index navigation [ J ]. IEEE International Conference on Electro/in.," 2008:343-348 ", mvcslm algorithm, which is implemented by tracking feature information of corners on the ground, using monocular camera as the main sensor, was proposed by Celik et al, the university of loving state, usa. However, the method has poor adaptability, requires obvious angular points in a room, and is difficult to deal with complex environments, and the method is useless when the angular points are shielded by furniture or people.
The laser radar is a sensor for sensing the surrounding environment by emitting laser beams and receiving reflected signals, can acquire three-dimensional coordinates and reflected intensity information of laser scanning points, and is mainly applied to the field of unmanned driving. Along with the development of unmanned driving, the laser radar develops rapidly, and the laser radar has high precision, interference resistance and small volume, has small data volume compared with a camera, and can be used under a dim condition. These advantages make lidar more suitable for unmanned aerial vehicle's indoor relative positioning.
Disclosure of Invention
Technical problem to be solved
In order to avoid the defects of the prior art, the invention provides an indoor relative positioning method based on a laser radar, and overcomes the defects of the existing indoor relative positioning method. The method is applied to a random sampling consistency algorithm, extracts the wall boundary of the indoor environment based on the random sampling consistency algorithm, and obtains the plane equation of the wall boundary model, thereby further obtaining the position information of the laser radar relative to the indoor environment.
Technical scheme
An indoor relative positioning method based on laser radar is characterized by comprising the following steps:
step 1: recording A, B on two adjacent walls respectively in a square indoor environment, randomly placing a laser radar at a certain indoor point, turning on the laser radar after electrifying, and acquiring multiframe laser radar point cloud data containing three-dimensional coordinate information and reflected echo intensity;
step 2: performing first fitting processing on each frame of laser radar point cloud data based on a random sampling consistency algorithm to obtain a plane equation of each frame of fitting wall surface A, and further obtaining the distance from the laser radar to the wall surface A; the treatment process comprises the following steps:
1. counting the frequency of the reflected echo intensity values in the laser radar point cloud data, selecting j groups of intensity values with the highest frequency, and recording as I1,I2,I3,...,IjPerforming through filtering on the echo intensity values, and reserving point cloud data corresponding to the echo intensity values as a sampled total sample set U1；
2. Randomly selecting 3 point cloud data from the samples obtained in the first step as a subset, and according to the selected subset data, satisfying the normalization condition on the first-order coefficient of the plane equation
Under the condition of (1), obtaining a plane equation a of the three points by using a undetermined coefficient method1x+b1y+c1z+d1＝0；
3. Setting an error margin1Define a consistent set Ω1Sequentially solving all sample points to a plane a as phi is an empty set1x+b1y+c1z+d1The distance of 0, the sample point A is recordedi(xi,yi,zi) To the plane a1x+b1y+c1z+d1Distance value of 0 is Li＝|a1xi+b1yi+c1zi+d1If the distance LiThe conditions are satisfied: l isi＜1Then sample point A is setiMark as local point and put it into consistent set omega1(ii) a Consistent set omega1The number of the element(s) is marked as k, and the model parameter of the consistent set omega is a1,b1,c1,d1；
4. Randomly selecting 3 point cloud data as a new subset, and solving a plane equation a of the new subset by using a undetermined coefficient method under the condition that the normalization condition is met1'x+b1'y+c1'z+d1' -0, according to the new subset plane equation a selected1'x+b1'y+c1'z+d1' 0, with the same toleranceLimit of1Under the limitation of (2), all the sample points are sequentially solved to the plane a again1'x+b1'y+c1'z+d1Distance L of ═ 0i', taken out to satisfy the condition Li'＜1And to classify these points into a new consistent set omega1', the number of elements of the consistent set is marked as k ', whether k ' is larger than k is judged, if so, the coefficient a of the plane equation is used1,b1,c1,d1Is updated to a1',b1',c1',d1', will simultaneously agree with a set omega1Updated to omega1', consistent set size k is updated to k'; if not, the plane equation coefficient a1,b1,c1,d1Consistent set omega1And the size k of the consistent set are kept unchanged;
5. setting an upper limit N of iteration times1Repeating step 4 and randomly sampling N1Then, output N1Sub-sampled uniform set omega1And model parameters a of the consistent set1,b1,c1,d1If the distance from the laser radar to the wall surface A is | d1|；
And step 3: from the total sample set U1Removing a consistent set omega output in the step 2 from the data points in the data processing system1Taking the obtained data points as a new sample set, and recording the new sample set as U2For sample set U2Repeating the random sampling consistency algorithm of step 2, setting a new error margin2And upper limit of iteration number N2Output the coincidence set omega at this time2And model parameters a of the consistent set2,b2,c2,d2Obtaining the distance | d of the laser radar from the wall surface B2|；
And 4, step 4: according to the results obtained in the step 2 and the step 3, a coordinate system is established in the normal vector direction of the adjacent wall surfaces, so that the indoor environment is kept in the first quadrant of the coordinate system; obtaining two-dimensional position coordinate information (x) obtained by processing point cloud data of each frame of laser radarj,yj) Wherein x isjDistance | d for representing j frame point cloud data output1|，yjRepresents the j frame pointDistance | d of cloud data output2|。
Advantageous effects
The indoor relative positioning method based on the laser radar processes the point cloud data of the laser radar based on the random sampling consistency algorithm, so as to obtain a plane equation of a wall surface A and a distance | d from the wall surface A1I, outputting a consistent set omega formed by data points contained in the wall surface A; removing elements in the consistent set omega output in the step 2 to form a new sample, and processing the point cloud data of the new sample based on a random sampling consistency algorithm again to obtain a plane equation of the adjacent wall surface B and a distance | d from the wall surface B2L, |; and (3) aiming at the two distances obtained in the step (2) and the step (3), establishing a proper coordinate system by taking the normal direction of the adjacent wall surfaces as a coordinate axis, so that the position coordinate information of the laser radar can be represented when the indoor environment is in the first quadrant.
Through the verification of a ground static experiment, the effects of the invention can be obtained as follows:
1. for a general indoor environment, the method can realize indoor relative positioning of the unmanned aerial vehicle, and has high precision which can reach 2cm (1 sigma);
2. the method is insensitive to shielding, even if shielding such as a large bookcase and the like which occupies about 50% of the area of the wall surface exists, the wall surface can be fitted as long as a flat wall surface exists, and the threshold value is reasonably selected, so that the position information of the unmanned aerial vehicle is obtained;
3. the method does not require the indoor environment to be static, and still can provide high-precision positioning if people or objects moving randomly exist in the room.
Due to the randomness of the random sampling consistency algorithm during sampling, a frame loss phenomenon possibly exists, namely, some frame of point cloud data cannot be fitted, but the influence of the limitation on a positioning result is small due to the high sampling frequency of the laser radar;
according to the graph, the vertical coordinate of the position changes within the range of 1cm, and the horizontal coordinate changes within the range of 3cm, so that the method can obtain very good precision when the unmanned aerial vehicle is positioned indoors.
Drawings
FIG. 1 is a graph comparing a random sampling consistency algorithm with a least squares method;
FIG. 2 is a flow chart of a random sampling consistency algorithm (taking a planar model as an example)
FIG. 3 is a flow chart of an indoor relative positioning method based on a random sampling consistency algorithm
FIG. 4 is a schematic diagram of an indoor experimental scenario
FIG. 5 is a diagram showing the fitting results of wall surface A
FIG. 6 is a diagram showing the fitting results of wall surface B
FIG. 7 is a schematic diagram of multi-frame data acquisition under random motion of an indoor obstruction
FIG. 8 is a one-dimensional coordinate of the location of each frame of data
FIG. 9 is a frequency distribution histogram of one-dimensional positioning errors
FIG. 10 is a two-dimensional positioning coordinate scattergram
FIG. 11 is a two-dimensional positioning coordinate frequency distribution histogram
FIG. 12 is a frequency distribution histogram of positioning errors
FIG. 13 is a graph showing the results of indoor relative positioning
FIG. 14 is a frequency distribution histogram of indoor relative positioning results
Detailed Description
The invention will now be further described with reference to the following examples and drawings:
the technical scheme of the invention mainly comprises the following steps: and (4) researching a random sampling consistency algorithm, and finishing indoor relative positioning based on the random sampling consistency algorithm. First, a brief description of a classical random sampling consistency algorithm is presented.
The stochastic sampling consistency algorithm is a model parameter estimation method, originally proposed by Fischler and Bolles in 1981, and is a resampling technique, in which model parameters are solved by estimating sample points obtained from stochastic sampling to obtain a model, the number of the sample points conforming to the solved model is calculated again, and if the number reaches a threshold value, the calculation is finished.
The basic assumptions of the random sampling consistency algorithm are:
1. the data consists of "local points" and "local points";
2. the data distribution of the local points can be explained by using some model parameters;
3. except the local point, other data are all regarded as the local point;
4. given a set of (usually small) local points, there is a process that can estimate the parameters of a model that can be interpreted or otherwise adapted to the local points.
For the convenience of understanding, the random sampling consistency algorithm is compared with the least square method, both of which are methods for fitting the model, but the least square method takes all data points into consideration, constructs an index (usually, a sum of squares of distance values), and a model parameter when the index reaches a maximum value is a fitting result. The random sampling consistency algorithm only divides data points into local inner points and local outer points to take all the points into consideration, and only carries out fitting processing on the local inner points. If the number of the local internal points exceeds the threshold value, the model parameters are output as a fitting result, otherwise, the sampling is carried out again until the data amount of the local internal points exceeds the threshold value or the sampling times exceeds the set upper limit.
The difference between the two is shown in fig. 1, the sample points are all data points, the black line is the result of least square fitting, and the sum of squares of distances from all points to the straight line is the minimum. The blue line is the result of fitting by a random sampling consistency algorithm, and the sample points are divided into two parts, namely local inner points (green points in the figure) and local outer points (red points in the figure) through random sampling, and only the local inner points are fitted.
By comparing the two methods, the applicable conditions for obtaining the random sampling consistency algorithm are as follows:
1. the input data is a group of observation data with noise data, wherein the credible data accounts for the most part;
2. there are models that can interpret trusted data.
The basic algorithm flow of the random sampling consistency algorithm is as follows:
1. randomly selecting a subset from the observation data, wherein the number of data points of the subset is not less than the minimum number required by the model calculation (generally equal to the minimum number required by the model calculation);
2. estimating model parameters suitable for the subset data points according to the selected subset data points;
3. defining an error allowable range, and obtaining data which accords with the model parameters in all the observation data in the error allowable range, wherein the data form a consistency set;
4. judging whether the number of consistency sets exceeds a set threshold value, if so, indicating that the estimation model is correct, and regarding the points in the consistency sets as local points and regarding other points as local points; if the threshold value is not exceeded, returning to the first step, and randomly selecting the subset again;
5. and re-fitting the local points by using a least square method or other fitting methods.
Taking the planar model as an example, the basic algorithm flow chart is shown in fig. 2.
The above is a classic random sampling consistency algorithm, which has two important parameters: the method comprises the steps of firstly, counting the number threshold value N in a local area, and secondly, performing an error tolerance function, namely an error range between an allowable data point and a fitting model. Obviously, the classical random sampling consistency algorithm cannot meet the positioning requirement. This is mainly because:
1. the complete random sampling consistency algorithm can only obtain one result at each time, and for two-dimensional indoor relative positioning, the distance from the unmanned aerial vehicle to two adjacent wall surfaces at least needs to be obtained, and the relative position coordinate of the unmanned aerial vehicle can be obtained according to the distance. Namely, a plurality of planes need to be fitted, that is, a consistency algorithm of multiple random sampling needs to be performed, and in this case, the local point number threshold N is difficult to select.
2. Along with unmanned aerial vehicle's motion, the quantity of the wall point that laser radar scanned can change, and this further increased the difficulty to choosing of interior point quantity threshold value N to lead to when unmanned aerial vehicle moves the wall limit, perhaps has great gesture change, can't fit out the plane model, thereby can't carry out indoor location to unmanned aerial vehicle.
There is therefore a need for an improved random sample consensus algorithm.
The above analysis shows that the problem is the choice of the threshold N, so that here we consider eliminating this threshold, which translates into a limitation on the number of random samples. The basic idea is as follows: the local point number threshold value N is not set, namely the comparison between the local point number of each iteration and the threshold value N is not considered, the maximum iteration number k is directly set, k groups of local point values are obtained after k iterations, and each group of data has Ni( i 1, 2.. k.) points, and finding the data n with the most local pointsjThe set of calculated results is considered as an optimal solution, and the set of local interior point models is considered as a final fitting result. And discarding the group of data after the theoretical fitting is finished, performing fitting calculation again on the new sample based on a random sampling consistency algorithm, and sequentially obtaining a plurality of fitting planes according to the method so as to obtain coordinate information of the laser radar.
In addition, considering that the echo intensity of the laser radar is related to materials, and generally in an indoor environment, the occupied area of the wall surface materials is the largest, and the echo intensity value is stable, it can be considered that the echo intensity value with high occurrence frequency is preferentially sampled when random sampling is performed, so that the calculation efficiency is improved.
In combination with the above analysis, the indoor relative positioning method based on the laser radar provided herein has the following steps:
step 1: counting the frequency of the reflected echo intensity values in the laser radar point cloud data, selecting j groups of intensity values with the highest frequency, and recording as I1,I2,I3,...,IjDesigning a through filter according to the echo intensity values, and reserving point cloud data corresponding to the echo intensity values as a sampled total sample set U1；
Step 2: randomly selecting 3 point cloud data from the samples obtained in the first step as a subset, and obtaining a plane equation a of the three points according to the selected subset data1x+b1y+c1z+d 10, wherein a1,b1,c1The normalization condition is satisfied: a is1 2+b1 2+c1 2＝1；
And step 3: setting an error margin1Define a consistent set Ω1Go through all sample points if sample point ai(xi,yi,zi) The conditions are satisfied: | a1xi+b1yi+c1zi+d1|＜1Then sample point A is setiMark as local point and put it into consistent set omega1. Consistent set omega1The number of the element(s) is marked as k, and the model parameter of the consistent set omega is a1,b1,c1,d1；
And 4, step 4: reselecting the 3 point cloud data as a new subset, and estimating a plane model a of the new subset1'x+b1'y+c1'z+d1' -0, according to the selected new subset model a1'x+b1'y+c1'z+d1' -0, within the same margin of error1Under the limitation of (2), traversing all sample points again to obtain a new consistent set omega1', the number of elements of the consistent set is marked as k ', whether k ' is larger than k is judged, if so, the model parameter a is used1,b1,c1,d1Is updated to a1',b1',c1',d1', will simultaneously agree with a set omega1Updated to omega1', consistent set size k is updated to k'; if not, the model parameter a1,b1,c1,d1Consistent set omega1And the size k of the consistent set are kept unchanged;
and 5: setting an upper limit N of iteration times1Repeating the fifth step and randomly sampling N1Then, output N1Sub-sampled uniform set omega1And model parameters a of the consistent set1,b1,c1,d1If the distance from the laser radar to the wall surface A is | d1|。
Step 6: taking a new sampleIs integrated into U2＝U1-Ω1I.e. the total sample set U1Removing a consistent set omega output in the step 2 from the data points in the data processing system1Taking the obtained data points as a new sample set, and recording the new sample set as U2Repeating the random sampling consistency algorithm of step 2 for a new sample set, setting a new error margin2And upper limit of iteration number N2Output the coincidence set omega at this time2And model parameters a of the consistent set2,b2,c2,d2And then the distance | d of the laser radar from the wall surface B can be obtained2|。
And 7: and (4) according to the results output in the steps 5 and 6, establishing a coordinate system in the normal vector direction of the adjacent wall surface, so that the indoor environment is kept in the first quadrant of the coordinate system. Obtaining two-dimensional position coordinate information (x) obtained by processing point cloud data of each frame of laser radarj，yj) Wherein x isjDistance | d for representing j frame point cloud data output1|，yjDistance | d for representing j frame point cloud data output2|。
The flow chart of the algorithm for extracting the wall model and the distance from the wall in the method is shown in figure 3.
Example 1: acquisition and processing of a frame of data in a stationary indoor environment
The experiment is selected to be carried out in a certain office, and the experimental scene is shown in figure 4. The laser radar is randomly placed on the desktop, and the coordinate information of the laser radar is determined by measuring the distance from the laser radar to the wall surface A and the wall surface B. In consideration of the characteristics of indoor environment, the experiment sets human shielding on the wall surface A, and is characterized in that the shielding is variable; the wall surface B is a wall surface shielded by a bookcase and is characterized by large shielding area but unchangeable shielding.
Data were collected, yielding a total of 55640 point cloud data points. The experimental data are processed by using an improved random sampling consistency algorithm for indoor relative positioning, considering that the measurement error of the laser radar is within 1cm (1 sigma), the measurement accuracy given by a radar parameter table is 2cm, and the wall surface is relatively flat, so that in order to improve the accuracy, the error tolerance is set to be 3cm, the iteration number is set to be N to 600, the previous steps 1-5 are executed, the wall surface a is successfully extracted, and the fitting result of the wall surface a is shown in fig. 5.
Wherein, the black dots are local point data judged by the algorithm, namely wall surface points, and the total number of the data points is 12288. And the green dots are local outlier data.
The standardized plane equation fitted to the wall surface a is:
0.8206x-0.5709y-0.0275z-2.7075＝0
therefore, the distance from the laser radar to the wall surface is 2.7075m, and the normal vector of the wall surface is
Discarding the local interior points after the initial fitting, executing step 6, and performing random sampling again to obtain a fitting result of the second wall surface, as shown in fig. 6.
4791 data points of wall B, the normalized plane equation obtained by fitting:
-0.5690x-0.8219y+0.0358z-4.7583＝0
according to the equation, the distance from the laser radar to the plane is 4.7583m, and the wall surface normal vector is
So far, the distance between the laser radar and two mutually perpendicular wall surfaces is obtained, step 7 is executed, a space rectangular coordinate system is established by taking the wall corner as an origin, and no definition is given
The direction is the direction of the x-axis,
the direction is the y-axis direction, and the position coordinates of the laser radar at this time are (2.7075m, 4.7583 m).
And the actual coordinates of the laser radar are (2.72m, 4.77m), and the method can be seen to have very high precision which can reach within 2 cm.
According to the above results, it can also be seen that the information of the wall model can still be obtained by the random sampling consistency algorithm under the condition that human occlusion exists. Under the condition of large-area shielding, the feature information of the wall model can still be successfully extracted.
Example 2: position coordinates are extracted from continuous multiframe laser radar point cloud data under indoor moving environment
When there is a moving person or object indoors, random occlusion is caused to the wall surface, so it is necessary to process continuous data to verify the applicability of the indoor relative positioning method based on random sampling consistency and evaluate the accuracy of the method when there is a moving person or object indoors.
The experimental scenario is shown in fig. 7.
For example, an experimenter freely walks in front of a wall surface, 40 frames of data collected by the laser radar are continuously processed, the distance from the laser radar to the wall surface A is obtained, distance information obtained by each frame of data is compared with real distance information, and the precision of the indoor positioning method can be obtained.
The distance d of the lidar from the wall surface a varies as shown in fig. 8.
The calculation yields a mean value of 3.0560m for d and a mean square error of 0.0029m for σ. And the true value of y is 3.04m, so that the frequency distribution histogram of the error can be plotted, as shown in fig. 9.
The figure shows that the positioning precision of the method can reach within 2 cm.
Due to the randomness of the random sampling consistency algorithm in sampling, the fitting of the wall surface has certain probability. The data points of the wall surface B are few, so that only 32 frame data obtain a parameter model of the wall surface B, the position information of the unmanned aerial vehicle obtained by the 32 frame data is drawn, and an indoor relative positioning result graph is obtained, as shown in fig. 10.
It can be easily found from the figure that the ordinate of the positioning changes within the range of 1cm and the abscissa changes around 3cm, so that the method can obtain very good precision for indoor positioning. Here, the abscissa positioning accuracy is slightly lower than the ordinate positioning accuracy because the abscissa is equal to the distance from the wall B, and the wall B has a large-area occlusion, although the wall model can still be fitted, the data points of the wall B are approximately equal to half of the data points of the wall a, which results in a slightly poor fitting result for the wall B.
A histogram of the frequency distribution in which the positioning result is made is shown in fig. 11.
As can be seen from the frequency distribution histogram, the indoor relative positioning result obtained by the method approximately follows normal distribution.
Meanwhile, random walk interference of people in a room exists in the data acquisition process of the experiment, and the insensitivity of the positioning method to moving objects is proved.
The invention applies a random sampling consistency algorithm to indoor relative positioning. The method comprises the steps of improving a classical random sampling consistency algorithm, processing indoor multi-line laser radar point cloud data by utilizing the improved random sampling consistency algorithm, fitting the multi-line laser radar point cloud data to obtain a wall surface equation of an indoor environment, and solving the position of a radar relative to the indoor environment. Through a ground static verification experiment, the positioning precision of the indoor relative positioning method based on random sampling consistency can reach 2cm, and the expected index requirement (0.1m) is met.
The method solves the problem of strong interference of indoor positioning, can cope with indoor complex environment, has small sensitivity to indoor environment change, can improve positioning efficiency by reasonably selecting parameters, and is suitable for being applied to indoor positioning engineering of unmanned aerial vehicles.
Claims (1)
1. An indoor relative positioning method based on laser radar is characterized by comprising the following steps:
step 1: recording A, B on two adjacent walls respectively in a square indoor environment, randomly placing a laser radar at a certain indoor point, turning on the laser radar after electrifying, and acquiring multiframe laser radar point cloud data containing three-dimensional coordinate information and reflected echo intensity;
step 2: performing first fitting processing on each frame of laser radar point cloud data based on a random sampling consistency algorithm to obtain a plane equation of each frame of fitting wall surface A, and further obtaining the distance from the laser radar to the wall surface A; the treatment process comprises the following steps:
1) counting the frequency of the reflected echo intensity values in the laser radar point cloud data, selecting j groups of intensity values with the highest frequency, and recording as I1,I2,I3,...,IjPerforming through filtering on the echo intensity values, and reserving point cloud data corresponding to the echo intensity values as a sampled total sample set U1；
2) Randomly selecting 3 point cloud data from the samples obtained in the first step as a subset, and according to the selected subset data, satisfying the normalization condition on the first-order coefficient of the plane equation
Under the condition of (1), obtaining a plane equation a of the three points by using a undetermined coefficient method1x+b1y+c1z+d1＝0；
3) Setting an error margin1Define a consistent set Ω1Sequentially solving all sample points to a plane a as phi is an empty set1x+b1y+c1z+d1The distance of 0, the sample point A is recordedi(xi,yi,zi) To the plane a1x+b1y+c1z+d1Distance value of 0 is Li＝|a1xi+b1yi+c1zi+d1If the distance LiThe conditions are satisfied: l isi＜1Then sample point A is setiMark as local point and put it into consistent set omega1(ii) a Consistent set omega1The number of the element(s) is marked as k, and the model parameter of the consistent set omega is a1,b1,c1,d1；
4) And randomly selecting 3 point cloud data as a new subset, and solving a new sub-set by using an undetermined coefficient method under the condition that the normalization condition is metSet of plane equations a1'x+b1'y+c1'z+d1' -0, according to the new subset plane equation a selected1'x+b1'y+c1'z+d1' -0, within the same margin of error1Under the limitation of (2), all the sample points are sequentially solved to the plane a again1'x+b1'y+c1'z+d1Distance L of ═ 0i', taken out to satisfy the condition Li'＜1And to classify these points into a new consistent set omega1', the number of elements of the consistent set is marked as k ', whether k ' is larger than k is judged, if so, the coefficient a of the plane equation is used1,b1,c1,d1Is updated to a1',b1',c1',d1', will simultaneously agree with a set omega1Updated to omega1', consistent set size k is updated to k'; if not, the plane equation coefficient a1,b1,c1,d1Consistent set omega1And the size k of the consistent set are kept unchanged;
5) setting an upper limit N of iteration times1Repeating step 4 and randomly sampling N1Then, output N1Sub-sampled uniform set omega1And model parameters a of the consistent set1,b1,c1,d1If the distance from the laser radar to the wall surface A is | d1|；
And step 3: from the total sample set U1Removing a consistent set omega output in the step 2 from the data points in the data processing system1Taking the obtained data points as a new sample set, and recording the new sample set as U2For sample set U2Repeating the random sampling consistency algorithm of step 2, setting a new error margin2And upper limit of iteration number N2Output the coincidence set omega at this time2And model parameters a of the consistent set2,b2,c2,d2Obtaining the distance | d of the laser radar from the wall surface B2|；
And 4, step 4: according to the results obtained in the step 2 and the step 3, a coordinate system is established in the normal vector direction of the adjacent wall surfaces, so that the indoor environment is kept in the first quadrant of the coordinate system; to obtain eachTwo-dimensional position coordinate information (x) obtained by processing one-frame laser radar point cloud dataj,yj) Wherein x isjDistance | d for representing j frame point cloud data output1|，yjDistance | d for representing j frame point cloud data output2|。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010876496.4A CN112099039B (en) | 2020-08-25 | 2020-08-25 | Indoor relative positioning method based on laser radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010876496.4A CN112099039B (en) | 2020-08-25 | 2020-08-25 | Indoor relative positioning method based on laser radar |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112099039A true CN112099039A (en) | 2020-12-18 |
| CN112099039B CN112099039B (en) | 2024-05-31 |
Family
ID=73757972
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010876496.4A Active CN112099039B (en) | 2020-08-25 | 2020-08-25 | Indoor relative positioning method based on laser radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112099039B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114637006A (en) * | 2022-05-07 | 2022-06-17 | 长沙莫之比智能科技有限公司 | Early warning area self-adaptive adjustment method based on millimeter wave personnel fall detection radar |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108171780A (en) * | 2017-12-28 | 2018-06-15 | 电子科技大学 | A kind of method that indoor true three-dimension map is built based on laser radar |
| CN110443836A (en) * | 2019-06-24 | 2019-11-12 | 中国人民解放军战略支援部队信息工程大学 | A kind of point cloud data autoegistration method and device based on plane characteristic |
| CN111090283A (en) * | 2019-12-20 | 2020-05-01 | 上海航天控制技术研究所 | Unmanned ship combined positioning and orientation method and system |
| CN111508022A (en) * | 2020-04-17 | 2020-08-07 | 无锡信捷电气股份有限公司 | Line laser stripe positioning method based on random sampling consistency |
-
2020
- 2020-08-25 CN CN202010876496.4A patent/CN112099039B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108171780A (en) * | 2017-12-28 | 2018-06-15 | 电子科技大学 | A kind of method that indoor true three-dimension map is built based on laser radar |
| CN110443836A (en) * | 2019-06-24 | 2019-11-12 | 中国人民解放军战略支援部队信息工程大学 | A kind of point cloud data autoegistration method and device based on plane characteristic |
| CN111090283A (en) * | 2019-12-20 | 2020-05-01 | 上海航天控制技术研究所 | Unmanned ship combined positioning and orientation method and system |
| CN111508022A (en) * | 2020-04-17 | 2020-08-07 | 无锡信捷电气股份有限公司 | Line laser stripe positioning method based on random sampling consistency |
Non-Patent Citations (2)
| Title |
|---|
| 李培华;马先哲;GUILLAUME LATHOUD: "基于有效帧估计和随机抽样一致性的集成目标定位方法", 黑龙江大学工程学报, vol. 2, no. 003, 31 December 2011 (2011-12-31) * |
| 陈韵;周军;: "基于激光雷达测量的空间交会对接相对导航", 航天控制, no. 01, 28 February 2006 (2006-02-28) * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114637006A (en) * | 2022-05-07 | 2022-06-17 | 长沙莫之比智能科技有限公司 | Early warning area self-adaptive adjustment method based on millimeter wave personnel fall detection radar |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112099039B (en) | 2024-05-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108596961B (en) | Point cloud registration method based on three-dimensional convolutional neural network | |
| CN108958282B (en) | Three-dimensional space path planning method based on dynamic spherical window | |
| EP2385496A1 (en) | Extraction of 2D surfaces from a 3D point cloud | |
| Henson et al. | Attitude-trajectory estimation for forward-looking multibeam sonar based on acoustic image registration | |
| WO2013106920A1 (en) | Densifying and colorizing point cloud representation of physical surface using image data | |
| CN107796384B (en) | 2D vehicle positioning using geographic arcs | |
| CN108917753B (en) | Aircraft position determination method based on motion recovery structure | |
| CN110243390B (en) | Pose determination method and device and odometer | |
| CN113470090A (en) | Multi-solid-state laser radar external reference calibration method based on SIFT-SHOT characteristics | |
| CN107798695B (en) | 3D vehicle positioning using geographic arcs | |
| Song et al. | Application of acoustic image processing in underwater terrain aided navigation | |
| CN104848861A (en) | Image vanishing point recognition technology based mobile equipment attitude measurement method | |
| CN114387462A (en) | Dynamic environment sensing method based on binocular camera | |
| CN109752698A (en) | A kind of inertial navigation method for estimating error of airborne synthetic aperture radar | |
| CN112099039B (en) | Indoor relative positioning method based on laser radar | |
| CN113838069B (en) | Point cloud segmentation method and system based on flatness constraint | |
| CN112017259B (en) | Indoor positioning and image building method based on depth camera and thermal imager | |
| CN117132737A (en) | Three-dimensional building model construction method, system and equipment | |
| Maki et al. | 3d model generation of cattle using multiple depth-maps for ict agriculture | |
| Kalyan et al. | FISST-SLAM: Finite set statistical approach to simultaneous localization and mapping | |
| CN113538579B (en) | Mobile robot positioning method based on unmanned aerial vehicle map and ground binocular information | |
| CN113129422A (en) | Three-dimensional model construction method and device, storage medium and computer equipment | |
| Dierenbach et al. | Next-Best-View method based on consecutive evaluation of topological relations | |
| CN118279515B (en) | Real-time multi-terminal map fusion method and device | |
| CN118379801B (en) | Sea surface rescue-oriented radar fusion human body action recognition system and 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 |