CN109270544A - Mobile robot self-localization system based on shaft identification - Google Patents
Mobile robot self-localization system based on shaft identification Download PDFInfo
- Publication number
- CN109270544A CN109270544A CN201811103396.7A CN201811103396A CN109270544A CN 109270544 A CN109270544 A CN 109270544A CN 201811103396 A CN201811103396 A CN 201811103396A CN 109270544 A CN109270544 A CN 109270544A
- Authority
- CN
- China
- Prior art keywords
- shaft
- point cloud
- grid
- information
- cloud data
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- 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
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a kind of Mobile robot self-localization systems based on shaft identification, it includes the following steps: to construct shaft offline map, the point cloud data around mobile object is obtained using laser radar, and shaft information is extracted from the point cloud data, it is self-positioning according to the matching of shaft information and shaft offline map progress, obtain the location information of mobile object.The present invention carries out recognition and tracking to shaft using laser radar, in conjunction with the information of Inertial Measurement Unit and GPS, the processing and fusion of data is carried out by corresponding location algorithm, to realize the self-positioning of mobile object.Method of the invention can achieve the positioning accuracy of decimeter grade, under the premise of meeting practical application request, there is that cost is relatively low, precision is very high, real-time and robustness are good.
Description
Technical field
The invention belongs to robot navigation's technical fields, are related to a kind of Mobile robot self-localization based on shaft identification
System such as identifies the method positioned to move vehicle based on shaft.
Background technique
When intelligent mobile robot or automatic driving vehicle are navigated, high-precision map is needed to go to reduce perception knowledge
Other error and noise bring are uncertain, while being matched by various kinds of sensors with map, and by calculating accordingly
Method carries out data processing to obtain itself accurate pose.
Currently used localization method is usually to cooperate Differential Global Positioning System using high-precision inertial measuring unit
(INS/DGPS), but this method has following disadvantage:
The cost of one, sensor itself is excessively high.A set of high-precision INS/DGPS system just needs up to ten thousand or even tens
Ten thousand cost, this is unacceptable for daily or industrial application mobile robot and automatic driving vehicle.
Secondly, due to GPS itself it is higher for environmental requirement, weather, position, block etc. due to will cause GPS's
Reliability reduces.
Alternatively, it is also possible to use the locating scheme based on laser radar or three-dimensional camera.The locating scheme is to pass through
The local feature region of itself or global characteristics of point cloud or image, and matched with map, it is fixed be largely calculated
Position.On the one hand the shortcomings that this method is the uncertainty for acquiring information characteristics, be on the other hand the spy due to cloud or image
Sign calculates and matching needs very big calculation amount, thus just bigger for the demand of processor hardware, and cost can also improve.
Locating scheme based on laser radar or three-dimensional camera is that the identification and matching based on road sign carry out, common
It is exactly positioned by the identification of traffic programme line, the applicable situation that essentially consists in of this method more limits to, it is necessary to be to have
The occasion of traffic programme line is not available in some rural roads, maintenance road or internal passageway.
Summary of the invention
In view of the deficiencies of the prior art, move vehicle positioned based on shaft identification the present invention provides a kind of
Method can overcome the shortcomings that being positioned using identification traffic programme line.Because of either which kind of road, both sides of the road one
As can all have more shaft, such as trees, street lamp, electric pole, road sign.Shaft is due to constant with generality, space-time
Property and readily identified feature, be a kind of to be quite suitable for map structuring and the road sign that is positioned of identification.The present invention is based on
This kind of shafts and realize positioning.After obtaining mark information, algorithm used for positioning common are Kalman filter
(KF), extended Kalman filter (EKF), maximal possibility estimation (MLE), particle filter (PF) and gaussian filtering etc..This
It is positioned in the positioning based on shaft identification that text proposes using having arrived particle filter, and has used Kalman filtering
Carry out the data fusion of multisensor.
To achieve the above object, present invention employs following technical solution principles are as follows:
A method of mobile object is positioned based on shaft identification comprising following steps:
(1), shaft offline map is constructed；
(2), the point cloud data around mobile object is obtained using laser radar, and extracts shaft from the point cloud data
Information；
(3), self-positioning according to the matching of shaft information and shaft offline map progress, obtain the positioning of mobile object
Information.
Wherein, in step (1), the construction method of shaft offline map is as follows:
1-1, the point cloud data set in a panel region is obtained using laser radar；
1-2, all point cloud datas for representing ground in point cloud data set are rejected using fitting ground Line Algorithm, obtained
Non-ground points cloud data；
1-3, non-ground points cloud data are converted to two-dimensional Cartesian system grid, contain the grid in each grid
Point cloud quantity, maximum height and minimum altitude, the size of grid are positively correlated with point cloud size in the grid；
1-4, cluster segmentation is carried out to the two-dimensional Cartesian system grid according to the point cloud quantity in each grid, by this
For two-dimensional Cartesian system mesh segmentation at multiple and different connected regions, each connected region includes multiple relevant grids；
1-5, cluster segmentation is carried out to different connected regions according to the maximum height information in each grid one by one, it will be every
Grid of a connected region inner height difference less than 20 centimetres is clustered as cloud, and obtains the width of each cloud cluster
Degree；Height difference is the difference of maximum height and minimum altitude；
1-6, rejecting maximum height putting lower than 70 centimetres, cloud clusters, width is clustered greater than the point cloud of maximum height；
1-7, the shaft information in the point cloud cluster after rejecting is extracted using round bar model algorithm, owned in a panel region
Shaft information just constitute shaft offline map.
In step (2), the point cloud data around mobile object is obtained using laser radar, and mention from the point cloud data
Take the process of shaft information as follows:
2-1, the point cloud data set around mobile object is obtained using laser radar；
2-2, all point cloud datas for representing ground in point cloud data set are rejected using fitting ground Line Algorithm, obtained
Non-ground points cloud data；
2-3, non-ground points cloud data are converted to two-dimensional Cartesian system grid, contain the grid in each grid
Point cloud quantity, maximum height and minimum altitude, the size of grid are positively correlated with point cloud size in the grid；
2-4, cluster segmentation is carried out to the two-dimensional Cartesian system grid according to the point cloud quantity in each grid, by this
For two-dimensional Cartesian system mesh segmentation at multiple and different connected regions, each connected region includes multiple relevant grids；
2-5, cluster segmentation is carried out to different connected regions according to the maximum height information in each grid one by one, it will be every
Grid of a connected region inner height difference less than 20 centimetres is clustered as cloud, and obtains the width of each cloud cluster
Degree；Height difference is the difference of maximum height and minimum altitude；
2-6, rejecting maximum height putting lower than 70 centimetres, cloud clusters, width is clustered greater than the point cloud of maximum height；
2-7, the shaft information in the point cloud cluster after rejecting is extracted using round bar model algorithm, as mobile object week
The shaft information enclosed.
Above-mentioned round bar model algorithm includes the following steps:
3-1, the point cloud cluster after rejecting is subjected to horizontal resection, the quantity of horizontal section is equal to the line number of laser radar；
3-2, the object for excluding non-circular section according to the circular geometry feature of each horizontal section and being not orthogonal to ground,
Obtain required shaft；
3-3, the location information at least obtaining required shaft and diameter information are as shaft information.
In step (3), matched at least with the location information and diameter information of shaft.
By adopting the above scheme, the beneficial effects of the present invention are:
The method positioned based on shaft identification to move vehicle of the invention is a kind of high-precision self-positioning side
Method, applicable object are mobile robot, automatic driving vehicle etc..In various traffic environments, the trees of both sides of the road, street lamp,
The shafts such as road sign have the characteristics that it is constant on space-time and identification when it is simple accurate, be it is a kind of be very suitable for positioning road
Mark.The present invention passes through phase in conjunction with the information of Inertial Measurement Unit and GPS using the recognition and tracking that laser radar carries out shaft
The location algorithm answered carries out the processing and fusion of data, to realize the self-positioning of mobile robot.Self-positioning side of the invention
Method can achieve the positioning accuracy of decimeter grade.Under the premise of meeting practical application request, have cost is relatively low, precision is very high,
Real-time and the good advantage of robustness.
Detailed description of the invention
Fig. 1 is localization method general flow chart of the invention.
Fig. 2 is shaft identification process figure of the invention.
Fig. 3 is the three-dimensional figure of the point cloud data set in a panel region of the invention.
Fig. 4 is that some clouds of the invention divide schematic diagram.
Fig. 5 is fan-shaped region schematic diagram of the invention.
Fig. 6 is the point cloud data effect picture rejected behind ground of the invention.
Fig. 7 is the equatorial projection figure after point cloud cluster segmentation of the invention.
Fig. 8 is the figure after column point cloud segmentation of the invention.
Fig. 9 is the X-Y scheme being correspondingly formed after column object point cloud of the invention is divided.
Figure 10 is the slice schematic diagram of shaft of the invention.
Figure 11 is approximate standard cylinder schematic diagram of the invention.
Figure 12 is the crown height of isolated tree point cloud of the invention, trunk is high and sets high schematic diagram.
Figure 13 is the hat width schematic diagram of isolated tree point cloud of the invention.
Figure 14 is the characteristic information figure of sign board of the invention.
Figure 15 is the figure of displaying pseudocode information of the invention.
Specific embodiment
The present invention provides a kind of methods for being identified based on shaft and being positioned to mobile object.This method is built upon
On the basis of establishing shaft map, extracting shaft information and position in real time, detail flowchart is as shown in Figure 1.The party
Method includes the following steps:
(1), shaft offline map is constructed；
(2), the point cloud data around mobile object is obtained using laser radar, and extracts shaft from the point cloud data
Information；
(3), it is positioned in real time according to shaft information and shaft offline map, obtains the positioning letter of mobile object
Breath.
Wherein, above-mentioned mobile object is move vehicle or mobile robot.
In step (1), building shaft offline map includes the process identified to shaft, the process it is detailed
Flow chart is as shown in Figure 2.The construction method of shaft offline map is as follows:
1-1, the point cloud data set in a panel region is obtained using laser radar；
1-2, all point cloud datas for representing ground in point cloud data set are rejected using fitting ground Line Algorithm, obtained
Non-ground points cloud data；
1-3, non-ground points cloud data are converted to horizontal two-dimensional Cartesian system grid, containing in each grid should
The information such as point cloud quantity, maximum height and the minimum altitude of grid, the size of grid are positively correlated with point cloud size in the grid；
1-4, cluster segmentation is carried out to the two-dimensional Cartesian system grid according to the point cloud quantity in each grid, by this
For two-dimensional Cartesian system mesh segmentation at multiple and different connected regions, each connected region includes multiple relevant grids；
1-5, cluster segmentation is carried out to different connected regions according to the maximum height information in each grid one by one, it will be every
Grid of a connected region inner height difference less than 20 centimetres is clustered as cloud, and obtains the width of each cloud cluster
Degree；Height difference is the difference of maximum height and minimum altitude；
1-6, rejecting maximum height putting lower than 70 centimetres, cloud clusters, width is clustered greater than the point cloud of maximum height, thus
Obtain the point cloud cluster of doubtful shaft；
1-7, shaft information in the point cloud cluster of doubtful shaft is extracted using round bar model algorithm, in a panel region
All shaft information just constitute shaft offline map.
In step 1-1, three-dimensional figure such as Fig. 3 institute of the point cloud data set in a panel region is obtained using laser radar
Show.
In step 1-2, after obtaining point cloud data set by laser radar, first carry out uncorrelated noise data filtering and
It rejects to improve shaft recognition efficiency, and first have to reject is all point clouds for representing ground, the present invention is using fitting ground
Face straight line is rejected to distinguish.The advantages of this method is that the small operation of calculation amount is fast, and precision is controllable, meets requirement and spy of the invention
Point.
As shown in Figure 4 and Figure 5, point cloud is first divided into n fan-shaped region (such as using mobile robot or move vehicle as the center of circle
S in figure0), which is further subdivided into m fritter, each cloud original coordinate p according to radiusi=(xi,yi,zi) one can be mapped to
On perpendicular, at this timeThen the three-dimensional point cloud in sector is mapped in a two-dimensional surface and draws
It is divided into m sections.Its height (i.e. z is taken in each sectioni) minimum point, the representative point p ' as this sectionbi。
Then according to delta algorithm, from the center of circle, substitution table point is for being fitted ground straight line one by one outward, wherein every increase by one
Straight line z that is a, being fittedi=mri+ b needs to meet claimed below: handling, can get rid of according to the pseudocode of Figure 15
The representative point on non-ground, and multistage fitting a straight line is obtained for representing ground.Wherein LsFor ground straight line set, LlastIn expression
The straight line once obtained, PsFor current replaced table point set.|Ps| indicate set element number.Wherein TsetIt indicates current to represent
The distance threshold value of point and a upper fitting a straight line, represents a little for rejecting the non-ground such as barrier, road shoulder；Tmset、Tbset、TerrPoint
Not Biao Shi the slope of fitting a straight line, intercept, fit standard difference limit value, for the segmentation of fitting a straight line to guarantee that ground fitting is straight
The precision of line.
After pseudocode shown in figure 15 processing, finally obtain one group of ground straight line, each at this time cloud computing its
In two-dimensional vertical plane at a distance from the straight line of ground, when distance is less than a setting value TgroundWhen, it regards as ground point cloud and picks
It removes.Point cloud data effect picture after rejecting ground is as shown in Figure 6.
The principle of step 1-4 is based on a cluster segmentation for cloud quantity, it is therefore an objective to find each grid according to cloud quantity
Neighbouring connection grid, multiple Connected network lattices are first beans-and bullets shooter cloud cluster at a connected region, thus non-conterminous object
Substantially it separates.Equatorial projection figure after the point cloud cluster segmentation of step 1-4 is as shown in Figure 7.
The principle of step 1-5 is the point cloud cluster segmentation based on height, it is therefore an objective to since 20 cm height differences, height
Point cloud projection grid of the degree difference greater than 20 centimetres separates again, the connected region conduct of (less than 20 centimetres) close with its height
One cluster, so that multiple cloud clusters are obtained, from its available width information of the two-dimensional shapes of each cluster.Column object point
Figure after cloud segmentation is as shown in Figure 8.The chart is bright after the point cloud cluster segmentation based on height, the point cloud with certain altitude
It can be divided out.The X-Y scheme that column object point cloud is correspondingly formed after being divided is as shown in Figure 9.According to the column object point of X-Y scheme
The two-dimensional shapes of cloud can obtain the width information of the column.
In step 1-6, after putting cloud cluster segmentation, the rejecting of noise object is first carried out, is filtered out as far as possible doubtful rod-shaped
Object.The object especially small for some cloud density, highly lower than 70 centimetres or object that width is also bigger than height can be with
It is rejected.In step 1-7, round bar model can be used to carry out the point cloud cluster after screening the matching and feature of shaft
Information extraction.Since present invention shaft detected is all perpendicular to ground, so base after cluster progress horizontal resection
It is calculated in circular geometry feature, and the horizontal section quantity clustered is consistent with the line number of laser radar.Circular geometry feature
It is as follows:
(x-cx)2+(y–cy)2=r2①
By arrangement, following form is obtained:
-r2+cx 2+cy 2–2xcx–2ycy=-(x2+y2)②
Being 2. applied in n point, following system of linear equations is obtained:
Ac=b
Wherein:
By solving this equation group, the solution in every three points available one group of center of circle and radius calculates a series of this solution
Standard deviation, by setting one threshold value non-circular section and be not orthogonal to ground object exclude.When standard deviation is less than
When threshold value, it is perpendicular to the round section of the object on ground.When standard deviation be greater than or equal to threshold value when, be non-circular section or
Belong to the object for being not orthogonal to ground, such case just belongs to non-shaft, needs to exclude.The each of a shaft is calculated
After the radius of a circle section and the center of circle, an approximate standard cylinder is obtained by calculating its average value, and be somebody's turn to do
The characteristic informations such as the location information and diameter information of approximate standard cylinder.Shaft be sliced as shown in Figure 10.It is logical
The obtained approximate standard cylinder of average value for crossing calculating circular slice is as shown in figure 11.
Wherein, the variance yields s of the diameter of multiple sections of doubtful shaft2It is the characteristic information being easy to get, it should
Characteristic information can distinguish rapidly open lamp and trees.And laser radar can also directly obtain the reflected intensity of a cloud, doubtful bar
The reflected intensity mean value of shape object area point cloud can reflect material information to a certain extent, can also be used as the feature being easy to get
Information.
After isolating single shaft, previously mentioned centre point coordinate (x, y) and diameter d, diameter variance yields are removed
s2, outside reflected intensity mean value, further other characteristic informations of the shaft can be extracted, such as the height and hat of tree
The characteristic informations such as the adjunct shape of width, street lamp or sign board.It should be pointed out that limit of such characteristic information due to its position
System (such as height), can have higher requirement, such as may require that using the more laser radars of line number, or need to merge to sensor
Other visual sensors such as camera etc..
Specifically, setting high measurement using its highest point, the high measurement of trunk utilizes point-cloud fitting for single plant trees
Height after cylinder, the measurement of hat width are then that the distribution projected using the level point cloud of trees centre point nearby coordinates is obtained.It is single
The crown height of strain tree point cloud, the information that trunk is high and tree is high are as shown in figure 12.In Figure 12, crown height indicates T1, the high table of trunk
Show T2, set high expression T, T=T1+T2.The hat width information of isolated tree point cloud is as shown in figure 13, and hat width indicates Tw。
For other feature information extractions of the standards cylinders such as street lamp, sign board bar: isolating single street lamp, mark
After board bar point cloud, other characteristic informations can also be further obtained, mainly its appendicular shape feature and position feature.Mark
The feature information extraction of will board bar etc. is as shown in figure 14.
By above processing, the characteristic informations such as the location information and diameter information of shaft are from raw sensor number
Extracted in, may be respectively used for based on shaft offline map building and it is self-positioning.
Above-mentioned shaft recognition methods is utilized in above-mentioned map structuring, is by equipped with high-precision inertia measurement
The measurement mobile robot or move vehicle of instrument and differential GPS (INS/DGPS) are according to above-mentioned shaft recognition methods, in mesh
Mark region carries out the map structuring based on shaft.The principle of map structuring are as follows: the characteristics of due to shaft, establish a two dimension
Shaft map, which is actually a sequence by shaft feature vector, the feature of each shaft to
Amount storage content includes: coordinate position, diameter value and characteristic information (such as body diameter variance, point cloud reflected intensity mean value, with
And the characteristic information of previously mentioned all kinds of shafts) etc..The selection of specific features information should be according to the line number of laser radar
And precision and the operational capability of controller select.Since the present invention more emphasizes real-time and low calculating cost, and apply
Particle filtering algorithm is of less demanding for characteristic matching, therefore uses feature vector herein are as follows: [coordinate position, diameter
Value, diameter variance, point cloud reflected intensity mean value], this feature vector for positioning when road sign matching provide it is enough
Particularity.In short, the present invention utilizes the sequence of one group of shaft feature vector, the shaft information in a panel region is described, with
For subsequent positioning, have the characteristics that storage content is few but is convenient for real-time road sign Rapid matching.
In step (2), the point cloud data around move vehicle is obtained using laser radar, and mention from the point cloud data
Take the process of shaft information as follows:
2-1, the point cloud data set around mobile object is obtained using laser radar；
2-2, all point cloud datas for representing ground in point cloud data set are rejected using fitting ground Line Algorithm, obtained
Non-ground points cloud data；
2-3, non-ground points cloud data are converted to two-dimensional Cartesian system grid, contain the grid in each grid
Point cloud quantity, maximum height and minimum altitude, the size of grid are positively correlated with point cloud size in the grid；
2-4, cluster segmentation is carried out to the two-dimensional Cartesian system grid according to the point cloud quantity in each grid, by this
For two-dimensional Cartesian system mesh segmentation at multiple and different connected regions, each connected region includes multiple relevant grids；
2-5, cluster segmentation is carried out to different connected regions according to the maximum height in each grid one by one, by each company
Logical grid of the inner height difference in region less than 20 centimetres is clustered as cloud, and obtains the width of each cloud cluster；It is high
Spend the difference that difference is maximum height and minimum altitude；
2-6, rejecting maximum height putting lower than 70 centimetres, cloud clusters, width is clustered greater than the point cloud of maximum height；
2-7, the shaft information in the point cloud cluster after rejecting is extracted using round bar model algorithm.
It requires to perceive shaft and identified in the building of offline map and position portion, and method is the same.
After the process of perception and the identification of shaft is principally obtaining point cloud data, is first filtered and simplified accordingly, further according to bar
The feature of shape object carries out the identification and segmentation of the point cloud cluster of shaft, finally obtains the characteristic information of shaft for building
Vertical offline map or positioning.And the map structuring based on shaft is the basis of positioning system of the invention, is removed necessary
Outside map rejuvenation, it is only necessary to implement to obtain offline map used for positioning in the initial stage.Positioning based on shaft is calculated
Method and Multisensor Data Fusion Algorithm are then operate in each mobile robot or vehicle termination, and progress is accurately made by oneself in real time
Position.
In step 3, using shaft information offline map as the mark information in world coordinate system, to recognize in real time
Shaft information as real-time measured information, realized on this basis according to the two information by particle filter algorithm accurate
Self-positioning, the displacement of the positioning result and Inertial Measurement Unit (IMU) that then particle filter are obtained using Kalman filtering
Information carries out data fusion, the location information of mobile object is obtained, to improve the real-time and robustness of positioning.
Particle filter algorithm includes the following steps:
(1), initialization positioning is carried out using common GPS.It is determined according to the worst error range of GPS and works as the big of prelocalization
Region is caused, which corresponds to specific target area in offline map.
(2), mobile robot or move vehicle model are established.In global reference coordinate, mobile robot or locomotive
Pose indicated with trivector: [x, y, θ].Wherein x, y respectively indicate mobile robot or move vehicle in horizontal two-dimension
Abscissa and ordinate under cartesian coordinate system, θ indicate robot just facing towards.
(3), particle collection is generated.Certain amount N is uniformly generated in the target area of the above-mentioned offline map having determined
Particle (being set as N=500 in test), the representation of particle is consistent with mobile robot or move vehicle model, and just
Beginningization particle weight:
(4), road sign matching is carried out.It is returned according to laser radar and handles to obtain shaft information, in offline map
Shaft storage information is matched.Match the information that uses include the diameter of shaft, the type of shaft, shaft it
Between the characteristic informations such as relative position.
(5), movement update is carried out.Movement, which updates, to be obtained by the inertia odometer of acquisition mobile robot or move vehicle
The pose variation delta odemetry=[Δ x, Δ y, Δ h] arrived, and the Gaussian noise Δ error added, variance yields depend on
In the precision of inertia odometer.More new particle position and posture.Shown in following formula:
(6) it is observed update comprising following steps:
The road sign measurement data for the laser radar that (6-1) is actually carried, including mobile robot or move vehicle with
The distance D of road sign, and observation angle α.
(6-2) by the comparison of offline map road sign position and particle position, the relatively corresponding road of available each particle
Target distance d and angle a.
(6-3) acquires weight for any road sign, by Gaussian function, and the weight acquired to all road signs is asked
Product, obtains the final weight ω [i] of particle.
(7) resampling is carried out.The present invention is carried out using SIR (Sample Importance Resampling) algorithm
Resampling, that is, carry out resampling immediately after each iteration, so that every time after iteration, so that particle, which is concentrated mainly on, more to be had
The region of possibility.Specific algorithm content repeats no more.
(8) position estimation module is carried out.It is last available by constantly recycling the process of the particle filter of (4) to (6)
The particle collection that one group of position is concentrated, pose is the estimation of mobile robot or move vehicle pose, by being averaged
The pose estimated result of mobile robot or move vehicle can be obtained.
The algorithm flow chart of above-mentioned shaft positioning is as shown in figure 15.
Since the positioned update frequency on common Vehicle Controller using particle filter only has 20~30Hz, therefore can be with
Using Kalman filtering, the positioning result of particle filter and the displacement information of IMU are carried out data fusion, to obtain move vehicle
Location information.The data fusion makes positioned update frequency can achieve 100Hz or more.Specific kalman filter method is not
It is described in detail again.Briefly, the displacement information of IMU is used for forecast updating module, and the positioning result that particle filter obtains is used for
Measurement updaue module.
In short, the present invention provides a kind of mobile robot based on shaft identification and particle filter is self-positioning in high precision
Method is assisted with laser radar for main identification sensor device with inertial measuring unit and GPS, with segmentation, processing, identification
Shaft point cloud information is the core building figure and positioning in real time, using particle filter as the real-time location algorithm of mobile robot.This
The self-positioning principle of invention are as follows: after having carried out ground point cloud and having rejected, point cloud is projected to and carries out gridding in plane, it is then logical
It crosses height threshold and puts the initial gross separation that cloud density carries out cluster separation and shaft.And pass through the cylindrical geometry feature of shaft
It is matched, be further separated out shaft and obtain its position and diameter information.Finally also respectively to trees and artificial rod-shaped
Other features of object (street lamp, sign board) carry out identification and and obtain.And herein on, with obtained rod-shaped object point cloud position and
The basis that characteristic information is established as offline map and positioned in real time.Believed when building figure by the position and feature that store the shaft
Breath establishes the two-dimentional offline map based on shaft.In real time when positioning, by identical shaft points cloud processing method, obtain
Currently detected shaft information, and the matching of shaft road sign is carried out with offline map, it is easy to implement further based on grain
The location algorithm of son filtering.
Person skilled in the art obviously easily can make various modifications to these embodiments, and saying herein
Bright General Principle is applied in other embodiments without having to go through creative labor.Therefore, the present invention is not limited to here
Embodiment, those skilled in the art's announcement according to the present invention, improvement and modification made without departing from the scope of the present invention are all answered
This is within protection scope of the present invention.
Claims (5)
1. a kind of identify the method positioned to mobile object based on shaft, characterized by the following steps:
(1), shaft offline map is constructed；
(2), the point cloud data around mobile object is obtained using laser radar, and extracts shaft letter from the point cloud data
Breath；
(3), self-positioning according to the matching of the shaft information and shaft offline map progress, obtain mobile object
Location information.
2. according to the method described in claim 1, it is characterized by: in step (1), the construction method of shaft offline map
It is as follows:
1-1, the point cloud data set in a panel region is obtained using laser radar；
1-2, all point cloud datas for representing ground in the point cloud data set are rejected using fitting ground Line Algorithm, obtained
Non-ground points cloud data；
1-3, the non-ground points cloud data are converted to two-dimensional Cartesian system grid, contain the grid in each grid
Point cloud quantity, maximum height and minimum altitude, the size of grid are positively correlated with point cloud size in the grid；
1-4, cluster segmentation is carried out to the two-dimensional Cartesian system grid according to the point cloud quantity in each grid, by the two dimension
For cartesian coordinate system mesh segmentation at multiple and different connected regions, each connected region includes multiple relevant grids；
1-5, cluster segmentation is carried out to different connected regions according to the maximum height information in each grid one by one, by each company
Logical grid of the inner height difference in region less than 20 centimetres is clustered as cloud, and obtains the width of each cloud cluster；Institute
State the difference that height difference is maximum height and minimum altitude；
1-6, rejecting maximum height putting lower than 70 centimetres, cloud clusters, width is clustered greater than the point cloud of maximum height；
1-7, the shaft information in the point cloud cluster after rejecting, all bars in a panel region are extracted using round bar model algorithm
Shape object information just constitutes the shaft offline map.
3. according to the method described in claim 1, it is characterized by: obtaining mobile object using laser radar in step (2)
The point cloud data of surrounding, and the process of extraction shaft information is as described below from the point cloud data:
2-1, the point cloud data set around mobile object is obtained using laser radar；
2-2, all point cloud datas for representing ground in the point cloud data set are rejected using fitting ground Line Algorithm, obtained
Non-ground points cloud data；
2-3, the non-ground points cloud data are converted to two-dimensional Cartesian system grid, contain the grid in each grid
Point cloud quantity, maximum height and minimum altitude, the size of grid are positively correlated with point cloud size in the grid；
2-4, cluster segmentation is carried out to the two-dimensional Cartesian system grid according to the point cloud quantity in each grid, by the two dimension
For cartesian coordinate system mesh segmentation at multiple and different connected regions, each connected region includes multiple relevant grids；
2-5, cluster segmentation is carried out to different connected regions according to the maximum height in each grid one by one, by each connected region
Grid of the domain inner height difference less than 20 centimetres is clustered as cloud, and obtains the width of each cloud cluster；Difference in height
Value is the difference of maximum height and minimum altitude；
2-6, rejecting maximum height putting lower than 70 centimetres, cloud clusters, width is clustered greater than the point cloud of maximum height；
2-7, the shaft information in the point cloud cluster after rejecting is extracted using round bar model algorithm, around mobile object
Shaft information.
4. according to the method in claim 2 or 3, it is characterised in that: the round bar model algorithm includes the following steps:
3-1, the point cloud cluster after the rejecting is subjected to horizontal resection, the quantity of horizontal section is equal to the line of the laser radar
Number；
3-2, the object for excluding non-circular section according to the circular geometry feature of each horizontal section and being not orthogonal to ground, obtain
Required shaft；
3-3, the location information at least obtaining required shaft and diameter information are as shaft information.
5. according to the method described in claim 1, it is characterized by: in step (3), at least with the location information of shaft
It is matched with diameter information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811103396.7A CN109270544A (en) | 2018-09-20 | 2018-09-20 | Mobile robot self-localization system based on shaft identification |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811103396.7A CN109270544A (en) | 2018-09-20 | 2018-09-20 | Mobile robot self-localization system based on shaft identification |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109270544A true CN109270544A (en) | 2019-01-25 |
Family
ID=65197822
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811103396.7A Pending CN109270544A (en) | 2018-09-20 | 2018-09-20 | Mobile robot self-localization system based on shaft identification |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109270544A (en) |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109919237A (en) * | 2019-03-13 | 2019-06-21 | 武汉海达数云技术有限公司 | Points cloud processing method and device |
| CN110069993A (en) * | 2019-03-19 | 2019-07-30 | 同济大学 | A kind of target vehicle detection method based on deep learning |
| CN110068830A (en) * | 2019-03-27 | 2019-07-30 | 东软睿驰汽车技术（沈阳）有限公司 | A kind of method and device of vehicle location |
| CN110097047A (en) * | 2019-03-19 | 2019-08-06 | 同济大学 | A kind of vehicle checking method using single line laser radar based on deep learning |
| CN110780669A (en) * | 2019-07-29 | 2020-02-11 | 苏州博田自动化技术有限公司 | Forest robot navigation and information acquisition method |
| CN110806585A (en) * | 2019-10-16 | 2020-02-18 | 北京理工华汇智能科技有限公司 | Robot positioning method and system based on trunk clustering tracking |
| CN110864646A (en) * | 2019-11-28 | 2020-03-06 | 北京百度网讯科技有限公司 | Method and device for detecting a lifting lever |
| CN110988949A (en) * | 2019-12-02 | 2020-04-10 | 北京京东乾石科技有限公司 | Positioning method, positioning device, computer readable storage medium and mobile device |
| CN111145182A (en) * | 2019-12-30 | 2020-05-12 | 芜湖哈特机器人产业技术研究院有限公司 | Visual positioning three-dimensional point cloud segmentation method |
| CN111429574A (en) * | 2020-03-06 | 2020-07-17 | 上海交通大学 | Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion |
| CN111602171A (en) * | 2019-07-26 | 2020-08-28 | 深圳市大疆创新科技有限公司 | Point cloud feature point extraction method, point cloud sensing system and movable platform |
| WO2020199173A1 (en) * | 2019-04-03 | 2020-10-08 | 华为技术有限公司 | Localization method and localization device |
| CN112199459A (en) * | 2020-09-30 | 2021-01-08 | 深兰人工智能(深圳)有限公司 | 3D point cloud segmentation method and segmentation device |
| CN112764053A (en) * | 2020-12-29 | 2021-05-07 | 深圳市普渡科技有限公司 | Fusion positioning method, device, equipment and computer readable storage medium |
| JP2021077018A (en) * | 2019-11-07 | 2021-05-20 | 東芝テック株式会社 | Point group data processing device |
| CN112904395A (en) * | 2019-12-03 | 2021-06-04 | 青岛慧拓智能机器有限公司 | Mining vehicle positioning system and method |
| JP6931501B1 (en) * | 2021-05-24 | 2021-09-08 | 株式会社マプリィ | Single tree modeling system and single tree modeling method |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2013064733A (en) * | 2011-08-29 | 2013-04-11 | Hokkaido Univ | Columnar object extraction method, columnar object extraction program and columnar object extraction device |
| JP2014153336A (en) * | 2013-02-13 | 2014-08-25 | Nippon Telegr & Teleph Corp <Ntt> | Point group analysis processing device and point group analysis processing program |
| CN105513127A (en) * | 2015-12-25 | 2016-04-20 | 武汉大学 | Rod-shaped object regular three-dimensional modeling method and rod-shaped object regular three-dimensional modeling system based on density peak clustering |
| CN105701478A (en) * | 2016-02-24 | 2016-06-22 | 腾讯科技（深圳）有限公司 | Method and device for extraction of rod-shaped ground object |
| CN105787445A (en) * | 2016-02-24 | 2016-07-20 | 武汉迈步科技有限公司 | Method and system for automatically extracting rod-shaped objects in vehicular laser scanning data |
-
2018
- 2018-09-20 CN CN201811103396.7A patent/CN109270544A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2013064733A (en) * | 2011-08-29 | 2013-04-11 | Hokkaido Univ | Columnar object extraction method, columnar object extraction program and columnar object extraction device |
| JP2014153336A (en) * | 2013-02-13 | 2014-08-25 | Nippon Telegr & Teleph Corp <Ntt> | Point group analysis processing device and point group analysis processing program |
| CN105513127A (en) * | 2015-12-25 | 2016-04-20 | 武汉大学 | Rod-shaped object regular three-dimensional modeling method and rod-shaped object regular three-dimensional modeling system based on density peak clustering |
| CN105701478A (en) * | 2016-02-24 | 2016-06-22 | 腾讯科技（深圳）有限公司 | Method and device for extraction of rod-shaped ground object |
| CN105787445A (en) * | 2016-02-24 | 2016-07-20 | 武汉迈步科技有限公司 | Method and system for automatically extracting rod-shaped objects in vehicular laser scanning data |
Non-Patent Citations (1)
| Title |
|---|
| 伍舜喜等: "基于自然柱状特征地图的智能车定位", 《上海交通大学学报》 * |
Cited By (28)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109919237A (en) * | 2019-03-13 | 2019-06-21 | 武汉海达数云技术有限公司 | Points cloud processing method and device |
| CN110069993B (en) * | 2019-03-19 | 2021-10-08 | 同济大学 | Target vehicle detection method based on deep learning |
| CN110069993A (en) * | 2019-03-19 | 2019-07-30 | 同济大学 | A kind of target vehicle detection method based on deep learning |
| CN110097047A (en) * | 2019-03-19 | 2019-08-06 | 同济大学 | A kind of vehicle checking method using single line laser radar based on deep learning |
| CN110068830A (en) * | 2019-03-27 | 2019-07-30 | 东软睿驰汽车技术（沈阳）有限公司 | A kind of method and device of vehicle location |
| CN112543877B (en) * | 2019-04-03 | 2022-01-11 | 华为技术有限公司 | Positioning method and positioning device |
| WO2020199173A1 (en) * | 2019-04-03 | 2020-10-08 | 华为技术有限公司 | Localization method and localization device |
| US12001517B2 (en) | 2019-04-03 | 2024-06-04 | Huawei Technologies Co., Ltd. | Positioning method and apparatus |
| CN112543877A (en) * | 2019-04-03 | 2021-03-23 | 华为技术有限公司 | Positioning method and positioning device |
| WO2021016751A1 (en) * | 2019-07-26 | 2021-02-04 | 深圳市大疆创新科技有限公司 | Method for extracting point cloud feature points, point cloud sensing system, and mobile platform |
| CN111602171A (en) * | 2019-07-26 | 2020-08-28 | 深圳市大疆创新科技有限公司 | Point cloud feature point extraction method, point cloud sensing system and movable platform |
| CN110780669A (en) * | 2019-07-29 | 2020-02-11 | 苏州博田自动化技术有限公司 | Forest robot navigation and information acquisition method |
| CN110806585A (en) * | 2019-10-16 | 2020-02-18 | 北京理工华汇智能科技有限公司 | Robot positioning method and system based on trunk clustering tracking |
| CN110806585B (en) * | 2019-10-16 | 2021-10-19 | 北京理工华汇智能科技有限公司 | Robot positioning method and system based on trunk clustering tracking |
| JP2021077018A (en) * | 2019-11-07 | 2021-05-20 | 東芝テック株式会社 | Point group data processing device |
| JP7393184B2 (en) | 2019-11-07 | 2023-12-06 | 東芝テック株式会社 | Point cloud data processing device |
| CN110864646A (en) * | 2019-11-28 | 2020-03-06 | 北京百度网讯科技有限公司 | Method and device for detecting a lifting lever |
| CN110988949A (en) * | 2019-12-02 | 2020-04-10 | 北京京东乾石科技有限公司 | Positioning method, positioning device, computer readable storage medium and mobile device |
| CN112904395A (en) * | 2019-12-03 | 2021-06-04 | 青岛慧拓智能机器有限公司 | Mining vehicle positioning system and method |
| CN111145182A (en) * | 2019-12-30 | 2020-05-12 | 芜湖哈特机器人产业技术研究院有限公司 | Visual positioning three-dimensional point cloud segmentation method |
| CN111145182B (en) * | 2019-12-30 | 2022-05-27 | 芜湖哈特机器人产业技术研究院有限公司 | Visual positioning three-dimensional point cloud segmentation method |
| CN111429574A (en) * | 2020-03-06 | 2020-07-17 | 上海交通大学 | Mobile robot positioning method and system based on three-dimensional point cloud and vision fusion |
| CN112199459A (en) * | 2020-09-30 | 2021-01-08 | 深兰人工智能(深圳)有限公司 | 3D point cloud segmentation method and segmentation device |
| CN112764053A (en) * | 2020-12-29 | 2021-05-07 | 深圳市普渡科技有限公司 | Fusion positioning method, device, equipment and computer readable storage medium |
| CN112764053B (en) * | 2020-12-29 | 2022-07-15 | 深圳市普渡科技有限公司 | Fusion positioning method, device, equipment and computer readable storage medium |
| JP6931501B1 (en) * | 2021-05-24 | 2021-09-08 | 株式会社マプリィ | Single tree modeling system and single tree modeling method |
| JP2022179840A (en) * | 2021-05-24 | 2022-12-06 | 株式会社マプリィ | Individual tree modeling system and individual tree modeling method |
| WO2022249507A1 (en) * | 2021-05-24 | 2022-12-01 | 株式会社マプリィ | Individual tree modeling system and individual tree modeling method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109270544A (en) | Mobile robot self-localization system based on shaft identification | |
| CN106842231B (en) | A kind of road edge identification and tracking | |
| CN106204705B (en) | A kind of 3D point cloud dividing method based on multi-line laser radar | |
| CN110781827B (en) | Road edge detection system and method based on laser radar and fan-shaped space division | |
| CN109144072A (en) | A kind of intelligent robot barrier-avoiding method based on three-dimensional laser | |
| CN111220993B (en) | Target scene positioning method and device, computer equipment and storage medium | |
| CN110084272B (en) | Cluster map creation method and repositioning method based on cluster map and position descriptor matching | |
| CN101509781B (en) | Walking robot positioning system based on monocular cam | |
| CN104729485B (en) | A kind of vision positioning method based on vehicle-mounted panoramic image Yu streetscape map match | |
| JP4232167B1 (en) | Object identification device, object identification method, and object identification program | |
| CN112001958B (en) | Virtual point cloud three-dimensional target detection method based on supervised monocular depth estimation | |
| CN108564650B (en) | Lane tree target identification method based on vehicle-mounted 2D LiDAR point cloud data | |
| CN108171131B (en) | Improved MeanShift-based method for extracting Lidar point cloud data road marking line | |
| CN107305126A (en) | The data configuration of environmental map, its manufacturing system and preparation method and its more new system and update method | |
| CN110320504A (en) | A kind of unstructured road detection method based on laser radar point cloud statistics geometrical model | |
| CN109544612A (en) | Point cloud registration method based on the description of characteristic point geometric jacquard patterning unit surface | |
| CN107392929B (en) | Intelligent target detection and size measurement method based on human eye vision model | |
| CN110542908A (en) | laser radar dynamic object perception method applied to intelligent driving vehicle | |
| CN114119863A (en) | Method for automatically extracting street tree target and forest attribute thereof based on vehicle-mounted laser radar data | |
| CN104657968B (en) | Automatic vehicle-mounted three-dimensional laser point cloud facade classification and outline extraction method | |
| CN109461132B (en) | SAR image automatic registration method based on feature point geometric topological relation | |
| CN114764871B (en) | Urban building attribute extraction method based on airborne laser point cloud | |
| CN109541612A (en) | Self aligning system in robot chamber based on single line laser radar identification column | |
| CN111487643B (en) | Building detection method based on laser radar point cloud and near-infrared image | |
| WO2023060632A1 (en) | Street view ground object multi-dimensional extraction method and system based on point cloud data |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20190125 |