CN106646508A - Slope angle estimation method for slope region based on multiline laser radar - Google Patents

Slope angle estimation method for slope region based on multiline laser radar Download PDF

Info

Publication number
CN106646508A
CN106646508A CN201611052553.7A CN201611052553A CN106646508A CN 106646508 A CN106646508 A CN 106646508A CN 201611052553 A CN201611052553 A CN 201611052553A CN 106646508 A CN106646508 A CN 106646508A
Authority
CN
China
Prior art keywords
coordinate system
laser radar
line laser
coordinate
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201611052553.7A
Other languages
Chinese (zh)
Other versions
CN106646508B (en
Inventor
周超
曹志强
孟祥瑞
庞磊
刘希龙
王硕
谭民
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Automation of Chinese Academy of Science
Original Assignee
Institute of Automation of Chinese Academy of Science
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Institute of Automation of Chinese Academy of Science filed Critical Institute of Automation of Chinese Academy of Science
Priority to CN201611052553.7A priority Critical patent/CN106646508B/en
Publication of CN106646508A publication Critical patent/CN106646508A/en
Application granted granted Critical
Publication of CN106646508B publication Critical patent/CN106646508B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C9/00—Measuring inclination, e.g. by clinometers, by levels
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

The invention discloses a slope angle estimation method for slope region based on a multiline laser radar, and the method comprises the steps: obtaining original laser radar point cloud data DL of a given slope region; carrying out the coordinate transformation and filtering of the DL through combining with the relation of coordinate transformation from a multiline laser radar coordinate system OLXLYLZL to a robot coordinate system ORXRYRZR, and generating new laser radar point cloud data DS; carrying out the plane fitting through a PROSAC algorithm based on DS, and obtaining a slope surface equation; selecting three data points from the DS, and calculating a ground equation through the data points, wherein the distances between the data points and a plane XRORYR do not exceed a given threshold value dZ; and calculating a slope angle estimation result according to the slope surface equation and the ground equation. The method obtains the distance information of the slope region through the multiline laser radar, achieves the estimation of the slope angle based on a slope surface normal vector, improves the estimation quality of the slope angle, and provides technical support for the application of robot environment understanding.

Description

Towards the ramp angles method of estimation based on multi-line laser radar of sloped region
Technical field
The present invention relates to robot environment understand field, it is particularly a kind of towards sloped region based on multi-line laser radar Ramp angles method of estimation.
Background technology
With the continuous development of mobile robot and sensor technology, its application is also constantly being expanded.For machine People towards outdoor environment for, slope is generally existing.The quality of slope detection will directly influence robot task The performance of execution.Both at home and abroad researcher is made that certain research in terms of ramp angles estimation.In the information of sloped region Collection aspect, is perceived using stereoscopic camera or depth camera more, but the distance obtained by stereoscopic camera or depth camera Information is often not accurate enough, it reduces the quality of ramp angles estimation.It is many by finding slope longitudinal cross-section in method Mode is calculating the angle on slope, and the method is more directly perceived, but due to the impact of the factor such as slope position and domatic direction, longitudinal direction The determination in section is relatively difficult, and this accuracy of estimation to ramp angles causes large effect.Compared with the feature of longitudinal cross-section, Slope this global characteristics of domatic normal vector can more effectively reflect slope.Can obtain more in view of multi-line laser radar Accurate range information, it is necessary to obtain the range information of sloped region using multi-line laser radar and be based on the domatic normal direction in slope Amount realizes the estimation of ramp angles.
The content of the invention
In order to solve the problems referred to above of the prior art, the accuracy of estimation for solving prior art to ramp angles has been It is difficult to meet the problem of demand, the present invention proposes a kind of estimating based on the ramp angles of multi-line laser radar towards sloped region Meter method, improves the quality of ramp angles estimation.
A kind of ramp angles method of estimation based on multi-line laser radar towards sloped region proposed by the present invention, including Following steps：
Step S1, obtains the original laser radar cloud data D of given sloped regionL；
Step S2, with reference to multi-line laser radar coordinate system OLXLYLZLTo robot coordinate system ORXRYRZRCoordinate transform close System, to DLCoordinate transform and filtering process are carried out, new laser radar point cloud data D is generatedS；
Step S3, based on DS, plane fitting is carried out using PROSAC algorithms, obtain the domatic equation in slope；
Step S4, from DSIt is middle to choose 3 to plane XRORYRDistance be less than given threshold value dZData point, and utilize This group of data point calculation obtains ground equation；
Step S5, according to the domatic equation in the slope and the ground equation estimated result of ramp angles is calculated.
Preferably, step S3 is using the concrete grammar that PROSAC algorithms obtain the domatic equation in slope：
Step S31, from new laser radar point cloud data DSIn optional 3 data points, calculate a plane；
Step S32, asks for DSIn other data points to step S31 calculate gained plane distance, with corresponding distance In dHIn the range of data point as it is interior point constitute in point set；Wherein dHFor given threshold value；
Step S33, process is iterated to maximize interior point quantity to guide, wherein being used to determine in each iterative process 3 data points of new plane are chosen from last interior point set the inside, until interior point account for ratio a little exceed in default Point proportion threshold value λ reaches default iterations MK；
Step S34, it is domatic as slope using the plane that last time iteration is obtained, and export the domatic equation in slope.
Preferably, the circular of ramp angles is in step S5：
Wherein,For the estimated result of ramp angles；upFor the domatic normal vector in slope, by the domatic equation in slope in robot A is expressed as under coordinate systemSL·x+BSL·y+CSL·z+DSL=0, wherein ASL、BSL、CSL、DSLFor four of the domatic equation in slope Coefficient, so as to up=(ASL,BSL,CSL)；uxoyFor ground normal vector, ground equation is expressed as into A under robot coordinate systemGF· x+BGF·y+CGF·z+DGF=0, wherein AGF、BGF、CGF、DGFFor four coefficients of ground equation, so as to uxoy=(AGF,BGF, CGF)。
Preferably, to D in step S2LCoordinate transform and filtering process are carried out, new laser radar point cloud data D is generatedS's Method is：
Step S21, based on multi-line laser radar coordinate the coordinate conversion relation of robot coordinate system is tied to, to step S1 institute The original laser radar cloud data D of acquisitionLCoordinate transform is carried out, cloud data D under corresponding robot coordinate system is obtainedR；
Step S22, using statistical filtering to DRProcess is filtered, new laser radar point cloud data D is generatedS。
Preferably, using statistical filtering to DRProcess is filtered, concrete grammar includes：
Step S221, calculates DRIn each point to the average distance between its m nearest neighbor point；Wherein m is default nearest The quantity of adjoint point；
Step S222, calculates distance threshold dF：
dF=μ+δ σ
Wherein, μ is the average of Gaussian Profile Φ, and σ is the standard deviation of Gaussian Profile Φ, and δ is given coefficient；Φ is step The Gaussian Profile that the average distance obtained in S221 is met；
Step S223, with reference to the D calculated in step S221RIn each point to the average departure between its m nearest neighbor point From selection average distance is less than or equal to distance threshold dFPoint constitute new laser radar point cloud data DS。
Preferably, the multi-line laser radar coordinate system OLXLYLZLTo robot coordinate system ORXRYRZRCoordinate transform close It is to be：
Wherein, (xR,yR,zR) and (xL,yL,zL) it is respectively point in multi-line laser radar cloud data in robot coordinate It is ORXRYRZRWith multi-line laser radar coordinate system OLXLYLZLIn three-dimensional coordinate；RrIt is from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix；RdIt is from coordinate system OPXPYPZPTo coordinate system ODXDYDZD's The spin matrix of coordinate transform；T is multi-line laser radar coordinate system OLXLYLZLWith robot coordinate system ORXRYRZRBetween translation Matrix；
Multi-line laser radar coordinate system OLXLYLZLWith robot coordinate system ORXRYRZRIt is right-handed system, multi-line laser radar Coordinate system OLXLYLZLOrigin OL(xLO,yLO,zLO) it is located at the center of multi-line laser radar, ZLAxle is perpendicular to multi-line laser radar Baseplane and direction is upwards, XLAxle is perpendicular to ZLAxle and with multi-line laser radar front be consistent；Robot coordinate system ORXRYRZROrigin OR(xRO,yRO,zRO) select robot center subpoint on the ground, ZRAxle is flat perpendicular to robot bottom Face and direction is upwards, XRAxle is perpendicular to ZRAxle and it is consistent with robot motion direction；
Coordinate system OPXPYPZPOrigin OP(xPO,yPO,zPO) and multi-line laser radar coordinate system OLXLYLZLOrigin OL (xLO,yLO,zLO) overlap, ZPAxle is in multi-line laser radar coordinate system OLXLYLZLIn vector representation be：XPAxle and YPAxle is in multi-line laser radar coordinate system OLXLYLZLIn vector representation be respectively：With
Coordinate system ODXDYDZDFor robot coordinate system ORXRYRZRThrough the coordinate system that the translation transformation of-T is obtained.
Preferably, multi-line laser radar coordinate system OLXLYLZLWith robot coordinate system ORXRYRZRBetween translation matrix T Computational methods are：
Wherein, tx、ty、tzRespectively multi-line laser radar coordinate system OLXLYLZLOrigin and robot coordinate system ORXRYRZR Deviation of the origin on three coordinate components.
Preferably, from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix RrSpecially：
In obtain ground multi-line laser radar cloud data with not having sloping flat open barnyard, therefrom optional 3 Data point constitutes set ξg, and ground is further calculated in multi-line laser radar coordinate system OLXLYLZLUnder equation：AG·x +BG·y+CG·z+DG=0, wherein AG、BG、CG、DGIt is ground in multi-line laser radar coordinate system OLXLYLZLUnder equation four Individual coefficient；
In coordinate system OLXLYLZLIn, plane XPOPYPWith plane XLOLYLThe vector of intersection be designated as N axles, be expressed as
The direction of rotation for defining α is from XPTo N axles, the direction of rotation of β is from Z to axlePAxle is to ZLAxle, the direction of rotation of γ is From N axle to XLAxle, from ZPAxle positive direction is looked over, when the direction of rotation of α is counterclockwise,OtherwiseLook over from the positive direction of N axles, when the direction of rotation of β is counterclockwise,OtherwiseFrom ZLThe positive direction of axle is looked over, and the direction of rotation of γ is When counterclockwise,Otherwise
Preferably, from coordinate system OPXPYPZPTo coordinate system ODXDYDZDCoordinate transform spin matrix RdSpecially：
The wherein acquisition methods of η are：
An elongated straight-bar is found described with not having sloping flat open barnyard, mobile straight-bar causes many line laser thunders Rare two lines are reached while measuring straight-bar, and detects the number of the point on the straight-bar measured by every line of straight-bar and be 1, the measurement data corresponding to the point on the straight-bar that multi-line laser radar is measured constitutes set ξP；
Using from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix Rr, From set ξPIn optional 2 data points transform to coordinate system OPXPYPZPUnder, straight-bar is calculated in coordinate system OPXPYPZPUnder straight line side Journey LP；
Using from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix Rr, Will set ξgIn 3 data points transform to coordinate system OPXPYPZPUnder, ground is calculated in coordinate system OPXPYPZPUnder plane equation PP；
According to LPAnd PP, the intersection point for calculating straight-bar with ground is in coordinate system OPXPYPZPUnder coordinate points PC(xPC,yPC,zPC)； Measurement obtains straight-bar with the intersection point on ground in coordinate system ODXDYDZDUnder coordinate points QC(xDC,yDC,zDC), connect PCWith PZ(0,0, zPC) obtain vectorConnection QCWith QZ(0,0,zDC) obtain vectorObtain vectorWithAngle η：
Preferably, the line number of multi-line laser radar is more than or equal to 4.
The present invention obtains the range information of sloped region and based on the domatic normal vector realization in slope using multi-line laser radar The estimation of ramp angles, improves the quality that ramp angles are estimated, the application for aspects such as robot environment's understandings provides technology Hold.
Description of the drawings
Fig. 1 is that the present invention illustrates towards the flow process of the ramp angles method of estimation based on multi-line laser radar of sloped region Figure.
Specific embodiment
With reference to the accompanying drawings describing the preferred embodiment of the present invention.It will be apparent to a skilled person that this A little embodiments are used only for explaining the know-why of the present invention, it is not intended that limit the scope of the invention.
A kind of ramp angles method of estimation based on multi-line laser radar towards sloped region of the present invention, including it is following Step：
Step S1, obtains the original laser radar cloud data D of given sloped regionL；
Step S2, with reference to multi-line laser radar coordinate system OLXLYLZLTo robot coordinate system ORXRYRZRCoordinate transform close System, to DLCoordinate transform and filtering process are carried out, new laser radar point cloud data D is generatedS；
To D in the stepLCoordinate transform and filtering process are carried out, new laser radar point cloud data D is generatedSMethod be：
Step S21, based on multi-line laser radar coordinate the coordinate conversion relation of robot coordinate system is tied to, to step S1 institute The original laser radar cloud data D of acquisitionLCoordinate transform is carried out, cloud data D under corresponding robot coordinate system is obtainedR；
Step S22, using statistical filtering to DRProcess is filtered, new laser radar point cloud data D is generatedS。
Using statistical filtering to D in the stepRProcess is filtered, concrete grammar includes：
Step S221, calculates DRIn each point to the average distance between its m nearest neighbor point；Wherein m is default nearest The quantity of adjoint point；
Step S222, calculates distance threshold dF, such as shown in formula (1)：
dF=μ+δ σ (1)
Wherein, μ is the average of Gaussian Profile Φ, and σ is the standard deviation of Gaussian Profile Φ, and δ is given coefficient；Φ is step The Gaussian Profile that the average distance obtained in S221 is met；
Step S223, with reference to the D calculated in step S221RIn each point to the average departure between its m nearest neighbor point From selection average distance is less than or equal to distance threshold dFPoint constitute new laser radar point cloud data DS。
Step S3, based on DS, plane fitting is carried out using PROSAC algorithms, obtain the domatic equation in slope；
The concrete grammar for obtaining the domatic equation in slope using PROSAC algorithms in the step is：
Step S31, from new laser radar point cloud data DSIn optional 3 data points, calculate a plane；
Step S32, asks for DSIn other data points (data points beyond 3 data points selected in step S31) The distance of gained plane is calculated to step S31, with corresponding distance in dHIn the range of data point as it is interior point constitute in point set； Wherein dHFor given threshold value；
Step S33, process is iterated to maximize interior point quantity to guide, wherein being used to determine in each iterative process 3 data points of new plane are chosen from last interior point set the inside, until interior point account for ratio a little exceed in default Point proportion threshold value λ reaches default iterations MK；
Step S34, it is domatic as slope using the plane that last time iteration is obtained, and export the domatic equation in slope；Slope Domatic equation is expressed as A under robot coordinate systemSL·x+BSL·y+CSL·z+DSL=0, wherein ASL、BSL、CSL、DSLFor oblique Four coefficients of the domatic equation in slope.
Step S4, from DSIt is middle to choose 3 to plane XRORYRDistance be less than given threshold value dZData point, and utilize This group of data point calculation simultaneously obtains ground equation；Ground equation is expressed as A under robot coordinate systemGF·x+BGF·y+CGF·z +DGF=0, wherein AGF、BGF、CGF、DGFFor four coefficients of ground equation.
Step S5, according to the domatic equation in the slope and the ground equation estimated result of ramp angles is calculated. Shown in the circular of ramp angles such as formula (2)：
Wherein,For the estimated result of ramp angles, up=(ASL,BSL,CSL) be the domatic normal vector in slope, uxoy=(AGF, BGF,CGF) it is ground normal vector.
In the present embodiment, multi-line laser radar coordinate system OLXLYLZLTo robot coordinate system ORXRYRZRCoordinate transform close System is as shown in formula (3)：
Wherein, (xR,yR,zR) and (xL,yL,zL) it is respectively point in multi-line laser radar cloud data in robot coordinate It is ORXRYRZRWith multi-line laser radar coordinate system OLXLYLZLIn three-dimensional coordinate；RrIt is from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix；RdIt is from coordinate system OPXPYPZPTo coordinate system ODXDYDZD's The spin matrix of coordinate transform；T is multi-line laser radar coordinate system OLXLYLZLWith robot coordinate system ORXRYRZRBetween translation Matrix；
Multi-line laser radar coordinate system OLXLYLZLWith robot coordinate system ORXRYRZRIt is right-handed system, multi-line laser radar Coordinate system OLXLYLZLOrigin OL(xLO,yLO,zLO) it is located at the center of multi-line laser radar, ZLAxle is perpendicular to multi-line laser radar Baseplane and direction is upwards, XLAxle is perpendicular to ZLAxle and with multi-line laser radar front be consistent；Robot coordinate system ORXRYRZROrigin OR(xRO,yRO,zRO) select robot center subpoint on the ground, ZRAxle is flat perpendicular to robot bottom Face and direction is upwards, XRAxle is perpendicular to ZRAxle and it is consistent with robot motion direction；
Coordinate system OPXPYPZPOrigin OP(xPO,yPO,zPO) and multi-line laser radar coordinate system OLXLYLZLOrigin overlap, ZPAxle is in multi-line laser radar coordinate system OLXLYLZLIn vector representation be：XPAxle and YPAxle is multi-thread Laser radar coordinate system OLXLYLZLIn vector representation be respectively：With
Coordinate system ODXDYDZDFor robot coordinate system ORXRYRZRThrough the coordinate system that the translation transformation of-T is obtained.
The concrete acquisition methods for being tied to the coordinate conversion relation of robot coordinate system to multi-line laser radar coordinate below enter Row is described in detail：
Note tx、ty、tzRespectively multi-line laser radar coordinate system OLXLYLZLOrigin OL(xLO,yLO,zLO) sit with robot Mark system ORXRYRZROrigin OR(xRO,yRO,zRO) deviation on three coordinate components, tx、ty、tzCan be obtained by measurement, from And obtain multi-line laser radar coordinate system OLXLYLZLWith robot coordinate system ORXRYRZRBetween translation matrix T such as formula (4) institute Show：
One piece of flat open barnyard ground (not having sloping barnyard ground) is selected, the multi-line laser radar point cloud on ground is obtained Data, therefrom optional 3 data points constitute set ξg。
Based on set ξgIn 3 data points be calculated ground in multi-line laser radar coordinate system OLXLYLZLUnder side Journey：AG·x+BG·y+CG·z+DG=0, wherein AG、BG、CG、DGIt is ground in multi-line laser radar coordinate system OLXLYLZLUnder Four coefficients of equation；In coordinate system OLXLYLZLIn, plane XPOPYPWith plane XLOLYLThe vector of intersection be designated as N axles, represent ForObtain from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform rotation Torque battle array RrAs shown in formula (5)：
The direction of rotation for defining α is from XPTo N axles, the direction of rotation of β is from Z to axlePAxle is to ZLAxle, the direction of rotation of γ is From N axle to XLAxle, from ZPAxle positive direction is looked over, when the direction of rotation of α is counterclockwise,OtherwiseLook over from the positive direction of N axles, when the direction of rotation of β is counterclockwise,OtherwiseFrom ZLThe positive direction of axle is looked over, and the direction of rotation of γ is When counterclockwise,Otherwise
Aforesaid flat open barnyard (do not have sloping barnyard ground) found an elongated straight-bar, mobile straight-bar is caused Multi-line laser radar at least two lines measure straight-bar simultaneously, and detect on the straight-bar measured by every line of straight-bar The number of point is 1, and the measurement data corresponding to the point on the straight-bar that multi-line laser radar is measured constitutes set ξP。
Using from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix Rr, From set ξPIn optional 2 data points transform to coordinate system OPXPYPZPUnder, straight-bar is calculated in coordinate system OPXPYPZPUnder straight line side Journey LP。
Using from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix Rr, Will set ξgIn 3 data points transform to coordinate system OPXPYPZPUnder, ground is calculated in coordinate system OPXPYPZPUnder plane equation PP。
According to LPAnd PP, the intersection point for calculating straight-bar with ground is in coordinate system OPXPYPZPUnder coordinate points PC(xPC,yPC,zPC)； Measurement obtains straight-bar with the intersection point on ground in coordinate system ODXDYDZDUnder coordinate points QC(xDC,yDC,zDC), connect PCWith PZ(0,0, zPC) obtain vectorConnection QCWith QZ(0,0,zDC) obtain vectorObtain vectorWithAngle η As shown in formula (6)：
From coordinate system OPXPYPZPTo coordinate system ODXDYDZDCoordinate transform spin matrix RdAs shown in formula (7)：
Then, obtain from multi-line laser radar coordinate system OLXLYLZLTo robot coordinate system ORXRYRZRCoordinate transform such as Shown in formula (3).
In the present embodiment, be ensure to give sloped region ramp angles estimation effect, the line of multi-line laser radar Number is more than or equal to 4.
In the present embodiment, following parameter value can be done：dH=0.03 meter, MK=50, λ=0.9, dz=0.03 meter, m=30, δ=2.0.
Those skilled in the art should be able to recognize that, with reference to the side of each example of the embodiments described herein description Method step, can with electronic hardware, computer software or the two be implemented in combination in, in order to clearly demonstrate electronic hardware and The interchangeability of software, according to function has generally described the composition and step of each example in the above description.These Function is performed with electronic hardware or software mode actually, depending on the application-specific and design constraint of technical scheme. Those skilled in the art can use different methods to realize described function to each specific application, but this reality Now it is not considered that beyond the scope of this invention.
Can be with hardware, computing device with reference to the method for the embodiments described herein description or the step of algorithm Software module, or the combination of the two is implementing.Software module can be placed in random access memory (RAM), internal memory, read-only storage (ROM), electrically programmable ROM, electrically erasable ROM, register, hard disk, moveable magnetic disc, CD-ROM or technical field In any other form of storage medium well known to interior.
So far, technical scheme is described already in connection with preferred embodiment shown in the drawings, but, this area Technical staff is it is easily understood that protection scope of the present invention is expressly not limited to these specific embodiments.Without departing from this On the premise of the principle of invention, those skilled in the art can make the change or replacement of equivalent to correlation technique feature, these Technical scheme after changing or replacing it is fallen within protection scope of the present invention.

Claims (10)

1. a kind of ramp angles method of estimation based on multi-line laser radar towards sloped region, it is characterised in that include with Lower step：
Step S1, obtains the original laser radar cloud data D of given sloped regionL；
Step S2, with reference to multi-line laser radar coordinate system OLXLYLZLTo robot coordinate system ORXRYRZRCoordinate conversion relation, it is right DLCoordinate transform and filtering process are carried out, new laser radar point cloud data D is generatedS；
Step S3, based on DS, plane fitting is carried out using PROSAC algorithms, obtain the domatic equation in slope；
Step S4, from DSIt is middle to choose 3 to plane XRORYRDistance be less than given threshold value dZData point, and using the group Data point calculation obtains ground equation；
Step S5, according to the domatic equation in the slope and the ground equation estimated result of ramp angles is calculated.
2. method according to claim 1, it is characterised in that step S3 obtains the domatic equation in slope using PROSAC algorithms Concrete grammar be：
Step S31, from new laser radar point cloud data DSIn optional 3 data points, calculate a plane；
Step S32, asks for DSIn other data points to step S31 calculate gained plane distance, with corresponding distance in dHModel Data point in enclosing constitutes interior point set as interior point；Wherein dHFor given threshold value；
Step S33, process is iterated to maximize interior point quantity to guide, wherein being used to determine new putting down in each iterative process 3 data points in face are chosen from last interior point set the inside, until interior point account for ratio a little exceed default interior point ratio Example threshold value λ reaches default iterations MK；
Step S34, it is domatic as slope using the plane that last time iteration is obtained, and export the domatic equation in slope.
3. method according to claim 1, it is characterised in that the circular of ramp angles is in step S5：
Wherein,For the estimated result of ramp angles；upFor the domatic normal vector in slope, by the domatic equation in slope in robot coordinate A is expressed as under systemSL·x+BSL·y+CSL·z+DSL=0, wherein ASL、BSL、CSL、DSLFor four coefficients of the domatic equation in slope, So as to up=(ASL,BSL,CSL)；uxoyFor ground normal vector, ground equation is expressed as into A under robot coordinate systemGF·x+ BGF·y+CGF·z+DGF=0, wherein AGF、BGF、CGF、DGFFor four coefficients of ground equation, so as to uxoy=(AGF,BGF,CGF)。
4. method according to claim 1, it is characterised in that to D in step S2LCoordinate transform and filtering process are carried out, it is raw The laser radar point cloud data D of Cheng XinSMethod be：
Step S21, is tied to the coordinate conversion relation of robot coordinate system, to acquired in step S1 based on multi-line laser radar coordinate Original laser radar cloud data DLCoordinate transform is carried out, cloud data D under corresponding robot coordinate system is obtainedR；
Step S22, using statistical filtering to DRProcess is filtered, new laser radar point cloud data D is generatedS。
5. method according to claim 4, it is characterised in that using statistical filtering to DRIt is filtered process, concrete grammar Including：
Step S221, calculates DRIn each point to the average distance between its m nearest neighbor point；Wherein m is default nearest neighbor point Quantity；
Step S222, calculates distance threshold dF：
dF=μ+δ σ
Wherein, μ is the average of Gaussian Profile Φ, and σ is the standard deviation of Gaussian Profile Φ, and δ is given coefficient；Φ is in step S221 The Gaussian Profile that the average distance for obtaining is met；
Step S223, with reference to the D calculated in step S221RIn each point to the average distance between its m nearest neighbor point, select Average distance is selected less than or equal to distance threshold dFPoint constitute new laser radar point cloud data DS。
6. the method according to any one of Claims 1 to 5, it is characterised in that the multi-line laser radar coordinate system OLXLYLZLTo robot coordinate system ORXRYRZRCoordinate conversion relation be：
x R y R z R = R d R r x L y L z L + T
Wherein, (xR,yR,zR) and (xL,yL,zL) it is respectively point in multi-line laser radar cloud data in robot coordinate system ORXRYRZRWith multi-line laser radar coordinate system OLXLYLZLIn three-dimensional coordinate；RrIt is from multi-line laser radar coordinate system OLXLYLZL To coordinate system OPXPYPZPCoordinate transform spin matrix；RdIt is from coordinate system OPXPYPZPTo coordinate system ODXDYDZDCoordinate become The spin matrix for changing；T is multi-line laser radar coordinate system OLXLYLZLWith robot coordinate system ORXRYRZRBetween translation matrix；
Multi-line laser radar coordinate system OLXLYLZLWith robot coordinate system ORXRYRZRIt is right-handed system, multi-line laser radar coordinate It is OLXLYLZLOrigin OL(xLO,yLO,zLO) it is located at the center of multi-line laser radar, ZLAxle is flat perpendicular to multi-line laser radar bottom Face and direction is upwards, XLAxle is perpendicular to ZLAxle and with multi-line laser radar front be consistent；Robot coordinate system ORXRYRZR Origin OR(xRO,yRO,zRO) select robot center subpoint on the ground, ZRAxle is perpendicular to robot baseplane and direction Upwards, XRAxle is perpendicular to ZRAxle and it is consistent with robot motion direction；
Coordinate system OPXPYPZPOrigin OP(xPO,yPO,zPO) and multi-line laser radar coordinate system OLXLYLZLOrigin overlap, ZPAxle In multi-line laser radar coordinate system OLXLYLZLIn vector representation be：XPAxle and YPAxle is in many line lasers Radar fix system OLXLYLZLIn vector representation be respectively：With
Coordinate system ODXDYDZDFor robot coordinate system ORXRYRZRThrough the coordinate system that the translation transformation of-T is obtained.
7. method according to claim 6, it is characterised in that multi-line laser radar coordinate system OLXLYLZLWith robot coordinate It is ORXRYRZRBetween the computational methods of translation matrix T be：
T = t x t y t z
Wherein, tx、ty、tzRespectively multi-line laser radar coordinate system OLXLYLZLOrigin and robot coordinate system ORXRYRZROriginal Deviation o'clock on three coordinate components.
8. method according to claim 7, it is characterised in that from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix RrSpecially：
R r = c o s α - s i n α 0 s i n α cos α 0 0 0 1 1 0 0 0 c o s β - s i n β 0 s i n β cos β c o s γ - s i n γ 0 s i n γ cos γ 0 0 0 1
In obtain ground multi-line laser radar cloud data with not having sloping flat open barnyard, therefrom optional 3 data Point constitutes set ξg, and ground is further calculated in multi-line laser radar coordinate system OLXLYLZLUnder equation：AG·x+ BG·y+CG·z+DG=0, wherein AG、BG、CG、DGIt is ground in multi-line laser radar coordinate system OLXLYLZLUnder four of equation Coefficient；
In coordinate system OLXLYLZLIn, plane XPOPYPWith plane XLOLYLThe vector of intersection be designated as N axles, be expressed as
The direction of rotation for defining α is from XPTo N axles, the direction of rotation of β is from Z to axlePAxle is to ZLAxle, the direction of rotation of γ is from N axles To XLAxle, from ZPAxle positive direction is looked over, when the direction of rotation of α is counterclockwise,OtherwiseLook over from the positive direction of N axles, when the direction of rotation of β is counterclockwise,OtherwiseFrom ZLThe positive direction of axle is looked over, the direction of rotation of γ For it is counterclockwise when,Otherwise
9. method according to claim 8, it is characterised in that from coordinate system OPXPYPZPTo coordinate system ODXDYDZDCoordinate The spin matrix R of conversiondSpecially：
R d = c o s η - s i n η 0 s i n η cos η 0 0 0 1
The wherein acquisition methods of η are：
An elongated straight-bar is found described with not having sloping flat open barnyard, mobile straight-bar causes multi-line laser radar extremely Rare two lines measure straight-bar simultaneously, and it is 1 to detect the number of the point on the straight-bar measured by every line of straight-bar, Measurement data corresponding to point on the straight-bar that multi-line laser radar is measured constitutes set ξP；
Using from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix Rr, from collection Close ξPIn optional 2 data points transform to coordinate system OPXPYPZPUnder, straight-bar is calculated in coordinate system OPXPYPZPUnder linear equation LP；
Using from multi-line laser radar coordinate system OLXLYLZLTo coordinate system OPXPYPZPCoordinate transform spin matrix Rr, will collect Close ξgIn 3 data points transform to coordinate system OPXPYPZPUnder, ground is calculated in coordinate system OPXPYPZPUnder plane equation PP；
According to LPAnd PP, the intersection point for calculating straight-bar with ground is in coordinate system OPXPYPZPUnder coordinate points PC(xPC,yPC,zPC)；Measurement The intersection point that straight-bar is obtained with ground is in coordinate system ODXDYDZDUnder coordinate points QC(xDC,yDC,zDC), connect PCWith PZ(0,0,zPC) Obtain vectorConnection QCWith QZ(0,0,zDC) obtain vectorObtain vectorWithAngle η：
10. method according to claim 9, it is characterised in that the line number of multi-line laser radar is more than or equal to 4.
CN201611052553.7A 2016-11-24 2016-11-24 Slope angle estimation method facing slope area and based on multi-line laser radar Active CN106646508B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611052553.7A CN106646508B (en) 2016-11-24 2016-11-24 Slope angle estimation method facing slope area and based on multi-line laser radar

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611052553.7A CN106646508B (en) 2016-11-24 2016-11-24 Slope angle estimation method facing slope area and based on multi-line laser radar

Publications (2)

Publication Number Publication Date
CN106646508A true CN106646508A (en) 2017-05-10
CN106646508B CN106646508B (en) 2020-04-07

Family

ID=58811590

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611052553.7A Active CN106646508B (en) 2016-11-24 2016-11-24 Slope angle estimation method facing slope area and based on multi-line laser radar

Country Status (1)

Country Link
CN (1) CN106646508B (en)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108898672A (en) * 2018-04-27 2018-11-27 厦门维斯云景信息科技有限公司 A kind of semi-automatic cloud method making three-dimensional high-definition mileage chart lane line
CN109668543A (en) * 2019-01-22 2019-04-23 南京理工大学 Inclination measurement method based on laser radar
CN109848997A (en) * 2019-03-20 2019-06-07 杭州晶一智能科技有限公司 Gradient method for quick predicting in front of mobile robot based on the stereoscopic camera that has a down dip
CN109917786A (en) * 2019-02-04 2019-06-21 浙江大学 A kind of robot tracking control and system operation method towards complex environment operation
CN110770597A (en) * 2018-11-21 2020-02-07 深圳市大疆创新科技有限公司 Terrain prediction method, device and system of rotary microwave radar and unmanned aerial vehicle
CN111504267A (en) * 2020-04-20 2020-08-07 上海联适导航技术有限公司 Slope rectifying method, device and equipment based on GNSS single-antenna satellite
CN112255633A (en) * 2020-09-25 2021-01-22 中国矿业大学 Method for automatic unloading of unmanned dump truck in refuse landfill
CN112684803A (en) * 2021-03-11 2021-04-20 上海擎朗智能科技有限公司 Control method and device for mobile robot, mobile robot and storage medium
CN113093218A (en) * 2021-05-14 2021-07-09 汤恩智能科技(苏州)有限公司 Slope detection method, drive device, and storage medium
CN113484872A (en) * 2021-06-16 2021-10-08 国能榆林能源有限责任公司 Laser radar underground slope depth detection method and detection vehicle
CN115167512A (en) * 2022-07-25 2022-10-11 亿航智能设备（广州）有限公司 Ground slope detection method and device and computer-readable storage medium

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1764580A1 (en) * 2005-09-14 2007-03-21 C.R.F. Società Consortile per Azioni Method and system for recognizing the sign of the velocity of a vehicle and for estimating the road slope
CN101482616A (en) * 2008-08-13 2009-07-15 中国科学院电子学研究所 Topographic survey method
CN103440358A (en) * 2013-07-15 2013-12-11 北京控制工程研究所 Slope fitting method based on DEM (dynamic effect model) data

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1764580A1 (en) * 2005-09-14 2007-03-21 C.R.F. Società Consortile per Azioni Method and system for recognizing the sign of the velocity of a vehicle and for estimating the road slope
CN101482616A (en) * 2008-08-13 2009-07-15 中国科学院电子学研究所 Topographic survey method
CN103440358A (en) * 2013-07-15 2013-12-11 北京控制工程研究所 Slope fitting method based on DEM (dynamic effect model) data

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
ANDREW JOHNSON ET AL.: "Lidar-based Hazard Avoidance for Safe Landing on Mars", 《JOURNAL OF GUIDANCE CONTROL & DYNAMICS》 *
沈莹莹 等: "火星探测器基于机器视觉的坡度估计", 《电子设计工程》 *

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108898672A (en) * 2018-04-27 2018-11-27 厦门维斯云景信息科技有限公司 A kind of semi-automatic cloud method making three-dimensional high-definition mileage chart lane line
CN110770597A (en) * 2018-11-21 2020-02-07 深圳市大疆创新科技有限公司 Terrain prediction method, device and system of rotary microwave radar and unmanned aerial vehicle
CN109668543A (en) * 2019-01-22 2019-04-23 南京理工大学 Inclination measurement method based on laser radar
CN109917786A (en) * 2019-02-04 2019-06-21 浙江大学 A kind of robot tracking control and system operation method towards complex environment operation
CN109848997A (en) * 2019-03-20 2019-06-07 杭州晶一智能科技有限公司 Gradient method for quick predicting in front of mobile robot based on the stereoscopic camera that has a down dip
CN111504267A (en) * 2020-04-20 2020-08-07 上海联适导航技术有限公司 Slope rectifying method, device and equipment based on GNSS single-antenna satellite
CN111504267B (en) * 2020-04-20 2020-12-15 上海联适导航技术有限公司 Slope rectifying method, device and equipment based on GNSS single-antenna satellite
CN112255633B (en) * 2020-09-25 2022-03-01 中国矿业大学 Method for automatic unloading of unmanned dump truck in refuse landfill
CN112255633A (en) * 2020-09-25 2021-01-22 中国矿业大学 Method for automatic unloading of unmanned dump truck in refuse landfill
CN112684803A (en) * 2021-03-11 2021-04-20 上海擎朗智能科技有限公司 Control method and device for mobile robot, mobile robot and storage medium
CN112684803B (en) * 2021-03-11 2021-06-11 上海擎朗智能科技有限公司 Control method and device for mobile robot, mobile robot and storage medium
WO2022188539A1 (en) * 2021-03-11 2022-09-15 上海擎朗智能科技有限公司 Method and apparatus for controlling mobile robot, mobile robot, and storage medium
US11619948B2 (en) 2021-03-11 2023-04-04 Keenon Robotics Co., Ltd. Control method of mobile robot, mobile robot, and storage medium
CN113093218A (en) * 2021-05-14 2021-07-09 汤恩智能科技(苏州)有限公司 Slope detection method, drive device, and storage medium
CN113484872A (en) * 2021-06-16 2021-10-08 国能榆林能源有限责任公司 Laser radar underground slope depth detection method and detection vehicle
CN113484872B (en) * 2021-06-16 2024-01-09 国能榆林能源有限责任公司 Laser radar underground slope depth detection method and detection vehicle
CN115167512A (en) * 2022-07-25 2022-10-11 亿航智能设备（广州）有限公司 Ground slope detection method and device and computer-readable storage medium

Also Published As

Publication number Publication date
CN106646508B (en) 2020-04-07

Similar Documents

Publication Publication Date Title
CN106646508A (en) Slope angle estimation method for slope region based on multiline laser radar
CN104613904B (en) Comprehensive determination method for coefficient of roughness of rock mass structure surface
Abtew et al. Spatial Analysis for Monthly Rainfall in South Florida 1
CN108362308B (en) Mileage correction method for mobile laser measurement system by using tunnel circular seam
CN104080165B (en) A kind of Indoor Wireless Sensor Networks localization method based on TDOA
CN104535981A (en) Double-quantile estimation method for sea clutter Pareto amplitude distribution parameters
CN103729846B (en) LiDAR point cloud data edge detection method based on triangular irregular network
CN109543356A (en) Consider the ocean interior temperature-salinity structure remote sensing inversion method of Space atmosphere
CN103106632A (en) Fusion method of different-accuracy three-dimension point cloud data based on mean shift
CN110231620A (en) A kind of noise correlation system tracking filter method
CN109636912A (en) Tetrahedron subdivision finite element interpolation method applied to three-dimensional sonar image reconstruction
Bilsky et al. A maximum entropy reconstruction technique for tomographic particle image velocimetry
Yu et al. Correlation analysis of spatio-temporal images for estimating two-dimensional flow velocity field in a rotating flow condition
CN105021199A (en) LS (Least square)-based multi- model adaptive state estimation method and system
CN110874569B (en) Unmanned aerial vehicle state parameter initialization method based on visual inertia fusion
Dachauer et al. Aerodynamic roughness length of crevassed tidewater glaciers from UAV mapping
CN108226002B (en) Method for obtaining secondary porosity based on cutoff value of secondary porosity spectrum of scale electrography
CN109387872B (en) Surface multiple prediction method
CN104704396A (en) A method of processing gravity gradient data
CN114417601B (en) Method for quickly estimating mesoscale eddy underwater sound velocity field based on satellite altimeter data
Salah Geostatistical analysis of groundwater levels in the south Al Jabal Al Akhdar area using GIS
CN112986948B (en) Building deformation monitoring method and device based on InSAR technology
Ng et al. Estimating the finite time Lyapunov exponent from sparse Lagrangian trajectories
CN103927536B (en) A kind of method for rapidly positioning of the light spot image center based on derivative method
Morse et al. Ice deformation in the vicinity of the ice-core site at Taylor Dome, Antarctica, and a derived accumulation rate history

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