CN109614459B - Map structuring winding detection method and equipment applied to two-dimensional laser - Google Patents
Map structuring winding detection method and equipment applied to two-dimensional laser Download PDFInfo
- Publication number
- CN109614459B CN109614459B CN201910168523.XA CN201910168523A CN109614459B CN 109614459 B CN109614459 B CN 109614459B CN 201910168523 A CN201910168523 A CN 201910168523A CN 109614459 B CN109614459 B CN 109614459B
- Authority
- CN
- China
- Prior art keywords
- laser
- frame
- present
- present laser
- coordinate system
- 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.)
- Active
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
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
Abstract
The object of the present invention is to provide a kind of map structuring winding detection methods and equipment applied to two-dimensional laser, the present invention is directed to traditional winding detection method, and there are error hiding rate height, it is computationally intensive, the defects of real-time is poor, a kind of winding detection method based on two-dimensional laser is provided, this method directly utilizes two-dimensional laser to realize winding detection.The present invention is compared to the progress effect of the winding detection method based on image: not only calculation amount is small for this method, and real-time is high, and has rotational invariance in winding detection.
Description
Technical field
The present invention relates to computer field more particularly to a kind of map structuring winding detection methods applied to two-dimensional laser
And equipment.
Background technique
Winding detection module is SLAM(map structuring) a part indispensable in system, it is mainly responsible for judgement machine
Whether people reaches previous position, and the winding information that will test is supplied to rear end and is handled.SLAM system introduces winding
Detection can effectively eliminate the accumulative mileage error during SLAM, construct high-precision environmental map.
The winding of mainstream detects by the way of image retrieval at present, by extracting the characteristic point of image, by present frame
The characteristic point that characteristic point and history keyword frame extract is registrated, if the matching logarithm of characteristic point is greater than certain quantity,
Think winding occur.This method is influenced by environment and the selection of manual features point, computationally intensive there are error hiding height, in real time
Property is poor, and is only applicable to the immovable situation in visual angle when winding.
Summary of the invention
It is an object of the present invention to provide a kind of map structuring winding detection methods and equipment applied to two-dimensional laser.
According to an aspect of the invention, there is provided a kind of map structuring winding detection method applied to two-dimensional laser,
This method comprises:
Step S1 determines the principal direction of the present laser frame L of the laser radar of robot, by the principal direction pair of present laser
The neat x-axis for arriving laser coordinate system；
The present laser frame L points are difference according to the difference of laser measurement values in the laser coordinate system by step S2
Subclass,Indicate the number of the subclass；
Step S3 calculates the subclassCorresponding plane description；
Step S4 merges the plane descriptionGenerate the present laser frameHistogram F, wherein
Indicate each plane description vectorsLength:
；
Step S5, using matrix norm, to present laser frameHistogramWith in laser map history keyword frame it is straight
Fang TuSimilarity measurement is carried out, if the similarity that measurement obtains is greater than preset threshold, determines that winding occurs.
Further, in the above method, if the similarity that measurement obtains is greater than preset threshold, determine that winding, packet occurs
It includes:
Work as distanceLess than setting threshold value when, then determine occur winding, wherein plane description vectorsWith
Indicate histogramWithColumn vector, i.e. each subclass of laser frame LDescription vectors:
。
Further, in the above method, the present laser frame of the laser radar of robot is determinedPrincipal direction, will be current
The principal direction of laser snaps to laser coordinate systemAxis, comprising:
The present laser frame L of the laser radar of robot is obtained, and determines whether present laser frame L is key frame, if
It is key frame, determines the principal direction of the present laser frame L of laser radar, the principal direction of present laser frame L is snapped into laser and is sat
Mark systemThen axis terminates this step and jumps to step S2；If not key frame, repeats this step.
Further, in the above method, the present laser frame L of the laser radar of robot is obtained, and is determined current sharp
Whether light frame L is key frame, if key frame, the principal direction of the present laser frame L of laser radar is determined, by present laser frame L
Principal direction snap to the x-axis of laser coordinate system, then terminate this step and jump to step S2；If not key frame, repeats this
Step, comprising:
Step S111, obtain present laser frame L, according to the mileage between the current time of robot and a upper moment away from
From judging whether present laser frame is key frame, if key frame, step S112 is jumped to；Otherwise, this step is repeated；
Step S112 distributes a direction description vectors for present laser frame L[0,0,0,0] andStraight lineAcross the origin of present laser frame；
Step S113 calculates the theorem in Euclid space coordinate of each laser beam in present laser frame L, calculation formula
It is as follows:
,
Wherein,It indicates the in present laser frame LThe measured value of a laser beam,It indicates the in present laser frame LIt is a
Angle value of the observation relative to laser coordinate system；
Step S114 calculates each laser beam of present laser frame LNearest straight line, update the direction description vectors, wherein calculating process is as follows:
,
；
Step S115 determines the principal direction of present laser frame L according to the maximum value in the direction description vectors D, root
According to the principal direction of laserPresent laser frame L is rotated to the x-axis of laser coordinate system, is then terminated this step and is jumped to step S2,
Wherein, calculating process is as follows,WithRespectively indicate straight lineSlope and intercept:
,
；
；
。
It further,, will be described current sharp according to the difference of laser measurement values in the laser coordinate system in the above method
Light frame L points are different subclasses, comprising:
Present laser frame L is expressed as, whereinIt indicates the in present laser frame LThe measurement of a laser beam
Value,It indicates the in present laser frame LAngle value of a observation relative to laser coordinate system；
To each laser beam in present laser frame L according to its measured valueSize be grouped into different subclasses。
Further, in the above method, the subclass is calculatedCorresponding plane description vectors, comprising:
For the subclassInitialize a plane description vectorsIt is latticed with distribution one
Structural body；
For the subclassEach memberCalculate its position in European coordinate system:
；
Calculate each point?In grid, and store it inIn；
According toIn storage point setIt calculatesCovarianceAnd its feature vectorAnd characteristic value, wherein,
,
,
；
Distribute m=4 straight lineIt passes throughMean value, according to the calculated characteristic value of previous stepWith
Feature vector, calculateAffiliated subclass index:
,
,
Wherein,Indicate feature vector and straight lineThe distance between;
According to the index, updateValue:
。
Another side according to the present invention also provides a kind of map structuring winding detection device applied to two-dimensional laser, should
Equipment includes:
First device, the principal direction of the present laser frame L of the laser radar for determining robot, by the master of present laser
Direction snaps to the x-axis of laser coordinate system；
Second device, for the difference according to laser measurement values in the laser coordinate system, by the present laser frame L points
For different subclasses,Indicate the number of the subclass；
3rd device, for calculating the subclassCorresponding plane description vectors；
4th device, for merging the plane description vectorsGenerate the present laser frameHistogram F, whereinIndicate each plane description vectorsLength:
；
5th device, for utilizing matrix norm, to present laser frameHistogramWith history keyword in laser map
The histogram of frameSimilarity measurement is carried out, if the similarity that measurement obtains is greater than preset threshold, determines that winding occurs.
Further, in above equipment, the 5th device, for working as distanceLess than setting threshold value when, then
Determine that winding occurs, wherein plane description vectorsWithIndicate histogramWithColumn vector, i.e. the every height of laser frame L
ClassDescription vectors:
。
Further, in above equipment, the first device, the present laser frame of the laser radar for obtaining robot
L, and determine whether present laser frame L is key frame, if key frame, determine the main side of the present laser frame L of laser radar
To the principal direction of present laser frame L is snapped to laser coordinate systemAxis, the execution for then terminating the first device jump
It is executed to the second device；If not key frame, repeats the first device.
Further, in above equipment, the first device, including
One one device, for obtaining present laser frame L, according between the current time of robot and a upper moment
Chainage judges whether present laser frame is key frame, if key frame, jumps to the first two devices；Otherwise, repeat this
The execution of device one by one；
First two devices, for distributing a direction description vectors for present laser frame L[0,0,0,0] andStraight lineAcross the origin of present laser frame；
One or three device, for calculating the theorem in Euclid space coordinate of each laser beam in present laser frame L, meter
It is as follows to calculate formula:
,
Wherein,It indicates the in present laser frame LThe measured value of a laser beam,It indicates the in present laser frame LIt is a
Angle value of the observation relative to laser coordinate system；
One or four device, for calculating each laser beam of present laser frame LNearest straight line, update the direction and retouch
State vector D, wherein calculating process is as follows:
,
；
One or five device, for determining the principal direction of present laser frame L according to the maximum value in the direction description vectors D, according to the principal direction of laserThe x-axis that present laser frame L arrives laser coordinate system is rotated, the one or five device the is then terminated
Execution jumps to the second device and executes, wherein and calculating process is as follows,WithRespectively indicate straight lineSlope and cut
Away from:
,
；
；
。
Further, in above equipment, the second device, for present laser frame L to be expressed as,
In,It indicates the in present laser frame LThe measured value of a laser beam,It indicates the in present laser frame LA observation is opposite
In the angle value of laser coordinate system；To each laser beam in present laser frame L according to its measured valueSize be grouped into difference
Subclass。
Further, in above equipment, 3rd device, for being the subclassInitialize a plane description vectorsWith one latticed structural body of distribution；For the subclassEach memberIt calculates
Its position in European coordinate system:
；
Calculate each point?In grid, and store it inIn；According toIn deposit
The point set of storageIt calculatesCovarianceAnd its feature vectorAnd characteristic value, wherein,
,
,
；
Distribute m=4 straight lineIt passes throughMean value, according to the calculated characteristic value of previous stepWith
Feature vector, calculateAffiliated subclass index:
,
,
Wherein,Indicate feature vector and straight lineThe distance between;
According to the index, updateValue:
。
Another side according to the present invention also provides a kind of computer-readable medium, is stored thereon with computer-readable instruction,
The computer-readable instruction can be executed by processor to realize method described in any of the above embodiments.
Another side according to the present invention also provides a kind of equipment for handling in network equipment client information, the equipment packet
Include the memory for storing computer program instructions and the processor for executing program instructions, wherein when the computer journey
When sequence instruction is executed by the processor, triggers the equipment and execute method described in any of the above embodiments.
Compared with prior art, there are error hidings for tradition winding detection method involved in background technique by the present invention
The defects of rate is high, computationally intensive, and real-time is poor, provides a kind of winding detection method based on two-dimensional laser, this method is directly sharp
Winding detection is realized with two-dimensional laser.The present invention is compared to the progress effect of the winding detection method based on image: we
Not only calculation amount is small for method, and real-time is high, and has rotational invariance in winding detection.
Detailed description of the invention
By reading a detailed description of non-restrictive embodiments in the light of the attached drawings below, of the invention other
Feature, objects and advantages will become more apparent upon:
The laser radar that Fig. 1 shows one embodiment of the invention divides subclass schematic diagram；
Fig. 2 shows the rotary laser principal directions of one embodiment of the invention to x-axis schematic diagram；
Fig. 3 shows the map structuring winding detection method flow chart applied to two-dimensional laser of one embodiment of the invention.
The same or similar appended drawing reference represents the same or similar component in attached drawing.
Specific embodiment
Present invention is further described in detail with reference to the accompanying drawing.
In a typical configuration of this application, terminal, the equipment of service network and trusted party include one or more
Processor (CPU), input/output interface, network interface and memory.
Memory may include the non-volatile memory in computer-readable medium, random access memory (RAM) and/
Or the forms such as Nonvolatile memory, such as read-only memory (ROM) or flash memory (flash RAM).Memory is computer-readable Jie
The example of matter.
Computer-readable medium includes permanent and non-permanent, removable and non-removable media can be by any method
Or technology come realize information store.Information can be computer readable instructions, data structure, the module of program or other data.
The example of the storage medium of computer include, but are not limited to phase change memory (PRAM), static random access memory (SRAM),
Dynamic random access memory (DRAM), other kinds of random access memory (RAM), read-only memory (ROM), electricity
Erasable Programmable Read Only Memory EPROM (EEPROM), flash memory or other memory techniques, read-only disc read only memory (CD-ROM)
(CD-ROM), digital versatile disc (DVD) or other optical storage, magnetic cassettes, magnetic tape disk storage or other magnetic
Property storage equipment or any other non-transmission medium, can be used for storing and can be accessed by a computing device information.According to herein
Define, computer-readable medium does not include non-temporary computer readable media (transitory media), such as modulation data
Signal and carrier wave.
As shown in figure 3, the present invention provides a kind of map structuring winding detection method applied to two-dimensional laser, the method
Include:
Step S1, as shown in Fig. 2, the principal direction of the present laser frame L of the laser radar of robot is determined, by present laser
Principal direction snap to the x-axis of laser coordinate system；
Step S2, as shown in Figure 1, according to the difference of laser measurement values in the laser coordinate system, by the present laser
Frame L points are different subclasses,Indicate the number of the subclass；
Step S3 calculates the subclassCorresponding plane description vectors；
Step S4 merges the plane description vectorsGenerate the present laser frameHistogram F, wherein
Indicate the number of subclass,Indicate each plane description vectorsLength；
；
Step S5, using matrix norm, to present laser frameHistogramWith in laser map history keyword frame it is straight
Fang TuSimilarity measurement is carried out, if the similarity that measurement obtains is greater than preset threshold, determines that winding occurs.
Here, the present invention is for tradition winding detection method involved in background technique, there are error hiding rate height, calculating
The defects of amount is big, and real-time is poor, provides a kind of winding detection method based on two-dimensional laser, this method directly utilizes two-dimensional laser
Realize winding detection.The present invention is compared to the progress effect of the winding detection method based on image: this method not only calculates
Small, real-time height is measured, and there is rotational invariance in winding detection.
In one embodiment of map structuring winding detection method applied to two-dimensional laser of the invention, in step S5, if surveying
The similarity measured is greater than preset threshold, then determines that winding occurs, comprising:
Work as distanceLess than setting threshold value when, then determine occur winding, wherein plane description vectorsWith
Indicate histogramWithColumn vector, i.e. each subclass of laser frame LDescription vectors:
。
Here, if distanceLess than setting threshold value then SLAM attempt carry out winding；Otherwise step S1 is jumped to.
The present embodiment is by judging distanceWhether it is less than the threshold value of setting, can is with efficient, reliable judgement
No generation winding.
In one embodiment of map structuring winding detection method applied to two-dimensional laser of the invention, step S1 determines machine
The present laser frame of the laser radar of device peoplePrincipal direction, the principal direction of present laser is snapped into laser coordinate systemAxis,
Include:
Step S11, obtains the present laser frame L of the laser radar of robot, and determines whether present laser frame L is pass
Key frame determines the principal direction of the present laser frame L of laser radar, the principal direction of present laser frame L is snapped to if key frame
Laser coordinate systemThen axis terminates this step and jumps to step S2；If not key frame, repeats this step, to continue to obtain
Take present laser frame L.
Here, passing through the present laser frame L for being chosen for key frame, it is ensured that the reliability of subsequent winding detection.
In one embodiment of map structuring winding detection method applied to two-dimensional laser of the invention, step S11 obtains machine
The present laser frame L of the laser radar of device people, and determine whether present laser frame L is key frame, if key frame, determines and swash
The principal direction of present laser frame L, is snapped to the x-axis of laser coordinate system, then tied by the principal direction of the present laser frame L of optical radar
This step of beam jumps to step S2；If not key frame, repeats this step, comprising:
Step S111, obtain present laser frame L, according to the mileage between the current time of robot and a upper moment away from
From judging whether present laser frame is key frame, if key frame, step S112 is jumped to；Otherwise, this step is repeated；
Step S112 distributes a direction description vectors for present laser frame L[0,0,0,0] andStraight lineAcross the origin of present laser frame；
Step S113 calculates the theorem in Euclid space coordinate of each laser beam in present laser frame L, calculation formula
It is as follows:
,
Wherein,It indicates the in present laser frame LThe measured value of a laser beam,It indicates the in present laser frame LIt is a
Angle value of the observation relative to laser coordinate system；
Step S114 calculates each laser beam of present laser frame LNearest straight line, update the direction description vectors, wherein calculating process is as follows:
,
；
Step S115 determines the principal direction of present laser frame L according to the maximum value in the description vectors D, according to sharp
The principal direction of lightPresent laser frame L is rotated to the x-axis of laser coordinate system, is then terminated this step and is jumped to step S2, such as Fig. 2
It is shown, wherein calculating process is as follows, whereinWithRespectively indicate straight lineSlope and intercept:
,
；
；
。
In one embodiment of map structuring winding detection method applied to two-dimensional laser of the invention, step S2, according to institute
The difference for stating laser measurement values in laser coordinate system divides the present laser frame L for different subclasses,
In,Value be 9, comprising:
Present laser frame L is expressed as by step S21, whereinIt indicates the in present laser frame LA laser
The measured value of beam,It indicates the in present laser frame LAngle value of a observation relative to laser coordinate system；
Step S22, to each laser beam in present laser frame L according to its measured valueSize be grouped into different subclasses, as shown in Figure 1, accurately sorting out to realize.
In one embodiment of map structuring winding detection method applied to two-dimensional laser of the invention, step S3 calculates institute
State subclassCorresponding plane description vectors, comprising:
Step S31 is the subclassInitialize a plane description vectorsWith distribution one
Latticed structural body；
Step S32, for the subclassEach memberCalculate its position in European coordinate system:
,
Step S33 calculates each point?In grid, and store it inIn；
Step S34, according toIn storage point setIt calculatesCovarianceAnd its
Feature vectorAnd characteristic value, wherein,
,
,
；
Step S35 distributes m=4 straight lineIt passes throughMean value, according to the calculated characteristic value of previous stepAnd feature vector, calculateAffiliated subclass index:
,
,
Wherein,Indicate feature vector and straight lineThe distance between;
Step S36 is updated according to the result index of previous stepValue:
。
Another side according to the present invention also provides a kind of map structuring winding detection device applied to two-dimensional laser, should
Equipment includes:
First device, the principal direction of the present laser frame L of the laser radar for determining robot, by the master of present laser
Direction snaps to the x-axis of laser coordinate system；
Second device, for the difference according to laser measurement values in the laser coordinate system, by the present laser frame L points
For different subclasses,Indicate the number of the subclass；
3rd device, for calculating the subclassCorresponding plane description vectors；
4th device, for merging the plane description vectorsGenerate the present laser frameHistogram F, whereinIndicate each plane description vectorsLength:
；
5th device, for utilizing matrix norm, to present laser frameHistogramWith history keyword in laser map
The histogram of frameSimilarity measurement is carried out, if the similarity that measurement obtains is greater than preset threshold, determines that winding occurs.
Further, in above equipment, the 5th device, for working as distanceLess than setting threshold value when, then
Determine that winding occurs, whereinWithIndicate histogramWithColumn vector, i.e. each subclass of laser frame LDescription to
Amount:
。
Further, in above equipment, the first device, the present laser frame of the laser radar for obtaining robot
L, and determine whether present laser frame L is key frame, if key frame, determine the main side of the present laser frame L of laser radar
To the principal direction of present laser frame L is snapped to laser coordinate systemAxis, the execution for then terminating the first device jump
It is executed to the second device；If not key frame, repeats the first device.
Further, in above equipment, the first device, including
One one device, for obtaining present laser frame L, according between the current time of robot and a upper moment
Chainage judges whether present laser frame is key frame, if key frame, jumps to the first two devices；Otherwise, repeat this
The execution of device one by one；
First two devices, for distributing a direction description vectors for present laser frame L[0,0,0,0] andStraight lineAcross the origin of present laser frame；
One or three device, for calculating the theorem in Euclid space coordinate of each laser beam in present laser frame L, meter
It is as follows to calculate formula:
,
Wherein,It indicates the in present laser frame LThe measured value of a laser beam,It indicates the in present laser frame LIt is a
Angle value of the observation relative to laser coordinate system；
One or four device, for calculating each laser beam of present laser frame LNearest straight line, update the direction and retouch
State vector, wherein calculating process is as follows:
,
；
One or five device, for determining the principal direction of present laser frame L according to the maximum value in the direction description vectors D, according to the principal direction of laserThe x-axis that present laser frame L arrives laser coordinate system is rotated, the one or five device the is then terminated
Execution jumps to the second device and executes, wherein and calculating process is as follows,WithRespectively indicate straight lineSlope and cut
Away from:
,
；
；
。
Further, in above equipment, the second device, for present laser frame L to be expressed as,
In,It indicates the in present laser frame LThe measured value of a laser beam,It indicates the in present laser frame LA observation is opposite
In the angle value of laser coordinate system；To each laser beam in present laser frame L according to its measured valueSize be grouped into difference
Subclass。
Further, in above equipment, 3rd device, for being the subclassInitialize a plane description vectorsWith one latticed structural body of distribution；For the subclassEach memberIt calculates
Its position in European coordinate system:
；
Calculate each point?In grid, and store it inIn；According toIn deposit
The point set of storageIt calculatesCovarianceAnd its feature vectorAnd characteristic value, wherein,
,
,
；
Distribute m=4 straight lineIt passes throughMean value, according to the calculated characteristic value of previous stepWith
Feature vector, calculateAffiliated subclass index:
,
,
Wherein,Indicate feature vector and straight lineThe distance between;
According to the index, updateValue:
。
Another side according to the present invention also provides a kind of computer-readable medium, is stored thereon with computer-readable instruction,
The computer-readable instruction can be executed by processor to realize method described in any of the above embodiments.
Another side according to the present invention also provides a kind of equipment for handling in network equipment client information, the equipment packet
Include the memory for storing computer program instructions and the processor for executing program instructions, wherein when the computer journey
When sequence instruction is executed by the processor, triggers the equipment and execute method described in any of the above embodiments.
The detailed content of each equipment and storage medium embodiment of the invention, for details, reference can be made to the correspondences of each method embodiment
Part, here, repeating no more.
Compared with prior art, there are error hidings for tradition winding detection method involved in background technique by the present invention
The defects of rate is high, computationally intensive, and real-time is poor, provides a kind of winding detection method based on two-dimensional laser, this method is directly sharp
Winding detection is realized with two-dimensional laser.The present invention is compared to the progress effect of the winding detection method based on image: we
Not only calculation amount is small for method, and real-time is high, and has rotational invariance in winding detection.
Obviously, those skilled in the art can carry out various modification and variations without departing from the essence of the application to the application
Mind and range.In this way, if these modifications and variations of the application belong to the range of the claim of this application and its equivalent technologies
Within, then the application is also intended to include these modifications and variations.
It should be noted that the present invention can be carried out in the assembly of software and/or software and hardware, for example, can adopt
With specific integrated circuit (ASIC), general purpose computer or any other realized similar to hardware device.In one embodiment
In, software program of the invention can be executed to implement the above steps or functions by processor.Similarly, of the invention
Software program (including relevant data structure) can be stored in computer readable recording medium, for example, RAM memory,
Magnetic or optical driver or floppy disc and similar devices.In addition, some of the steps or functions of the present invention may be implemented in hardware, example
Such as, as the circuit cooperated with processor thereby executing each step or function.
In addition, a part of the invention can be applied to computer program product, such as computer program instructions, when its quilt
When computer executes, by the operation of the computer, it can call or provide according to the method for the present invention and/or technical solution.
And the program instruction of method of the invention is called, it is possibly stored in fixed or moveable recording medium, and/or pass through
Broadcast or the data flow in other signal-bearing mediums and transmitted, and/or be stored according to described program instruction operation
In the working storage of computer equipment.Here, according to one embodiment of present invention including a device, which includes using
Memory in storage computer program instructions and processor for executing program instructions, wherein when the computer program refers to
When enabling by processor execution, method and/or skill of the device operation based on aforementioned multiple embodiments according to the present invention are triggered
Art scheme.
It is obvious to a person skilled in the art that invention is not limited to the details of the above exemplary embodiments, Er Qie
In the case where without departing substantially from spirit or essential attributes of the invention, the present invention can be realized in other specific forms.Therefore, no matter
From the point of view of which point, the present embodiments are to be considered as illustrative and not restrictive, and the scope of the present invention is by appended power
Benefit requires rather than above description limits, it is intended that all by what is fallen within the meaning and scope of the equivalent elements of the claims
Variation is included in the present invention.Any reference signs in the claims should not be construed as limiting the involved claims.This
Outside, it is clear that one word of " comprising " does not exclude other units or steps, and odd number is not excluded for plural number.That states in device claim is multiple
Unit or device can also be implemented through software or hardware by a unit or device.The first, the second equal words are used to table
Show title, and does not indicate any particular order.
Claims (8)
1. a kind of map structuring winding detection method applied to two-dimensional laser, wherein this method comprises:
Step S1 determines the principal direction of the present laser frame L of the laser radar of robot, the principal direction of present laser is snapped to
The x-axis of laser coordinate system, comprising:
The present laser frame L of the laser radar of robot is obtained, and determines whether present laser frame L is key frame, if closing
Key frame, determines the principal direction of the present laser frame L of laser radar, and the principal direction of present laser frame L is snapped to laser coordinate system
X-axis, then terminate this step and jump to step S2；If not key frame, repeats this step；
Step S2 divides the present laser frame L for different sons according to the difference of laser measurement values in the laser coordinate system
Class Li, i=1...n, n=9 indicate the number of the subclass；
Step S3 calculates the subclass LiCorresponding plane description vectors Fi, comprising:
For the subclass LiInitialize a plane description vectors Fi=[0,0,0,0]TWith one latticed structural body β of distribution；
For the subclass LiEach member (di, θi) calculate its position (x in European coordinate systemi, yi):
Calculate each point (xi, yi) grid b in βi∈ β, and store it in biIn；
According to biPoint set { the X of storage in ∈ β1, X2..., XsCalculate biCovariance ∑iAnd its feature vector e and feature
Value λ, wherein
|∑i- λ E |=0；
Distribute m=4 straight line πJ=1 ..., mAcross biMean μi, according to the calculated eigenvalue λ of previous stepI=1,2With feature to
Measure eI=1,2, calculate biAffiliated subclass index:
Index=arg miniD (e, πi), i=1,2,3,4,
Wherein, d (e, πi) indicate feature vector and straight line πiThe distance between；
According to the index, F is updatedi(index) value:
Fi(index)=Fi(index)+1；
Step S4 merges the plane description vectors FiGenerate the histogram F of present laser frame L, wherein m=4 indicates each
Plane description vectors FiLength:
Step S5, using matrix norm, to the histogram G of history keyword frame in the histogram F and laser map of present laser frame L
Similarity measurement is carried out, if the similarity that measurement obtains is greater than preset threshold, determines that winding occurs, comprising:
When distance δ (F, G) is less than the threshold value of setting, then the similarity measured is greater than preset threshold, determines that winding occurs,
Wherein, plane description vectors FiAnd GiIndicate the column vector of histogram F and G:
2. according to the method described in claim 1, wherein, obtaining the present laser frame L of the laser radar of robot, and determining
Whether present laser frame L is key frame, if key frame, determines the principal direction of the present laser frame L of laser radar, will currently swash
The principal direction of light frame L snaps to the x-axis of laser coordinate system, then terminates this step and jumps to step S2；If not key frame, weight
Duplicate step, comprising:
Step S111 obtains present laser frame L, was sentenced according to the chainage between the current time of robot and a upper moment
Whether disconnected present laser frame is key frame, if key frame, jumps to step S112；Otherwise, this step is repeated；
Step S112 distributes a direction description vectors D=[0,0,0,0] and m=4 straight line π for present laser frame LJ=1 ..., m
Across the origin of present laser frame L；
Step S113 calculates the theorem in Euclid space coordinate X of each laser beam in present laser frame Li(xi, yi), calculation formula is as follows:
Wherein, diIndicate the measured value of i-th of laser beam in present laser frame L, θiIndicate i-th of observation in present laser frame L
Angle value relative to laser coordinate system；
Step S114 calculates each laser beam X of present laser frame LiNearest straight line πj, the direction description vectors D is updated,
In, calculating process is as follows:
Index=arg minj d(Xi, πj), j=1 ..., m,
D (index)=D (index)+1；
Step S115 determines the principal direction θ of present laser frame L according to the maximum value in the direction description vectors Dj, according to laser
Principal direction θjPresent laser frame L is rotated to the x-axis of laser coordinate system, then terminates this step and jumps to step S2, wherein meter
Calculation process is as follows, AjAnd bjRespectively indicate straight line πjSlope and intercept:
J=arg maxi Di, i=1...m,
πj: yj=Ajxj+bj；
θj=atan Aj；
3. according to the method described in claim 2, wherein, according to the difference of laser measurement values in the laser coordinate system, by institute
Present laser frame L points are stated as different subclass Li, i=1...n, comprising:
Present laser frame L is expressed as L (di, θi), wherein diIndicate the measured value of i-th of laser beam in present laser frame L, θi
Indicate angle value of i-th of observation relative to laser coordinate system in present laser frame L；
To each laser beam in present laser frame L according to its measured value diSize be grouped into different subclass Li。
4. a kind of map structuring winding detection device applied to two-dimensional laser, wherein the equipment includes:
First device, the principal direction of the present laser frame L of the laser radar for determining robot, by the principal direction of present laser
Snap to the x-axis of laser coordinate system, wherein the first device, the present laser frame of the laser radar for obtaining robot
L, and determine whether present laser frame L is key frame, if key frame, determine the main side of the present laser frame L of laser radar
To the principal direction of present laser frame L being snapped to the x-axis of laser coordinate system, the execution for then terminating the first device jumps
It is executed to the second device；If not key frame, repeats the first device；
Second device, for the difference according to laser measurement values in the laser coordinate system, by the present laser frame L points for not
Same subclass Li, i=1...n, n=9 indicate the number of the subclass；
3rd device, for calculating the subclass LiCorresponding plane description vectors Fi, wherein the 3rd device is used for as institute
State subclass LiInitialize a plane description vectors Fi=[0,0,0,0]TWith one latticed structural body β of distribution；For described
Subclass LiEach member (di, θi) calculate its position (x in European coordinate systemi, yi):
Calculate each point (xi, yi) grid b in βi∈ β, and store it in biIn；According to biThe point set of storage in ∈ β
Close { X1, X2..., XsCalculate biCovariance ∑iAnd its feature vector e and eigenvalue λ, wherein
|∑i- λ E |=0；
Distribute m=4 straight line πJ=1 ..., mAcross biMean μi, according to the calculated eigenvalue λ of previous stepI=1,2With feature to
Measure eI=1,2, calculate biAffiliated subclass index:
Index=arg miniD (e, πi), i=1,2,3,4,
Wherein, d (e, πi) indicate feature vector and straight line πiThe distance between；
According to the index, F is updatedi(index) value:
Fi(index)=Fi(index)+1；
4th device, for merging the plane description vectors FiGenerate the histogram F of present laser frame L, wherein m=4 table
Show each plane description vectors FiLength:
5th device, for utilizing matrix norm, to history keyword frame in the histogram F and laser map of present laser frame L
Histogram G carries out similarity measurement, if the similarity that measurement obtains is greater than preset threshold, determines that winding occurs, wherein described
5th device, when for being less than the threshold value of setting as distance δ (F, G), then the similarity measured is greater than preset threshold, determines
Winding occurs, wherein plane description vectors FiAnd GiIndicate the column vector of histogram F and G:
5. equipment according to claim 4, wherein the first device, comprising:
One one device, for obtaining present laser frame L, according to the mileage between the current time of robot and a upper moment
Whether Distance Judgment present laser frame is key frame, if key frame, jumps to the first two devices；Otherwise, sheet the 1st is repeated
The execution of device；
First two devices, for distributing a direction description vectors D=[0,0,0,0] and m=4 straight line for present laser frame L
πJ=1 ..., mAcross the origin of present laser frame L；
One or three device, for calculating the theorem in Euclid space coordinate X of each laser beam in present laser frame Li(xi, yi), calculation formula
It is as follows:
Wherein, diIndicate the measured value of i-th of laser beam in present laser frame L, θiIndicate i-th of observation in present laser frame L
Angle value relative to laser coordinate system；
One or four device, for calculating each laser beam X of present laser frame LiNearest straight line πj, update the direction describe to
Measure D, wherein calculating process is as follows:
Index=arg minj d(Xi, πj), j=1 ..., m,
D (index)=D (index)+1；
One or five device, for determining the principal direction θ of present laser frame L according to the maximum value in the direction description vectors Dj, root
According to the principal direction θ of laserjThe x-axis that present laser frame L arrives laser coordinate system is rotated, then terminates the one or five device the and executes jump
It goes to the second device to execute, wherein calculating process is as follows, AjAnd bjRespectively indicate straight line πjSlope and intercept:
J=arg maxi Di, i=1...m,
πj: yj=Ajxj+bj；
θj=atan Aj；
6. equipment according to claim 5, wherein the second device, for present laser frame L to be expressed as L (di,
θi), wherein diIndicate the measured value of i-th of laser beam in present laser frame L, θiIndicate i-th of observation in present laser frame L
Angle value relative to laser coordinate system；To each laser beam in present laser frame L according to its measured value diSize be grouped into
Different subclass Li。
7. a kind of computer-readable medium, is stored thereon with computer-readable instruction, the computer-readable instruction can be processed
Device is executed to realize the method described in any one of claims 1 to 3.
8. a kind of equipment for handling in network equipment client information, which includes for storing depositing for computer program instructions
Reservoir and processor for executing program instructions, wherein when the computer program instructions are executed by the processor, triggering should
Method described in any one of equipment perform claim requirement 1 to 3.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910168523.XA CN109614459B (en) | 2019-03-06 | 2019-03-06 | Map structuring winding detection method and equipment applied to two-dimensional laser |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910168523.XA CN109614459B (en) | 2019-03-06 | 2019-03-06 | Map structuring winding detection method and equipment applied to two-dimensional laser |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109614459A CN109614459A (en) | 2019-04-12 |
| CN109614459B true CN109614459B (en) | 2019-05-28 |
Family
ID=66022247
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910168523.XA Active CN109614459B (en) | 2019-03-06 | 2019-03-06 | Map structuring winding detection method and equipment applied to two-dimensional laser |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109614459B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110880010A (en) * | 2019-07-05 | 2020-03-13 | 电子科技大学 | Visual SLAM closed loop detection algorithm based on convolutional neural network |
| CN110796683A (en) * | 2019-10-15 | 2020-02-14 | 浙江工业大学 | Repositioning method based on visual feature combined laser SLAM |
| CN112162294B (en) * | 2020-10-10 | 2023-12-15 | 北京布科思科技有限公司 | Robot structure detection method based on laser sensor |
| CN112668653A (en) * | 2020-12-30 | 2021-04-16 | 北京百度网讯科技有限公司 | Loop detection method, device, equipment and medium based on laser radar map |
| CN113781569B (en) * | 2021-01-04 | 2024-04-05 | 北京京东乾石科技有限公司 | Loop detection method and device |
| CN113390370B (en) * | 2021-05-17 | 2023-01-13 | 珠海格力电器股份有限公司 | Object detection method and device, electronic equipment and storage medium |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN106056643B (en) * | 2016-04-27 | 2018-10-26 | 深圳积木易搭科技技术有限公司 | A kind of indoor dynamic scene SLAM method and system based on cloud |
| SG10201700299QA (en) * | 2017-01-13 | 2018-08-30 | Otsaw Digital Pte Ltd | Three-dimensional mapping of an environment |
| CN107316328B (en) * | 2017-05-11 | 2020-07-07 | 浙江大学 | Closed loop detection method based on angular point characteristics of two-dimensional laser scanner |
| CN107403163B (en) * | 2017-07-31 | 2019-11-08 | 武汉大学 | A kind of laser SLAM closed loop automatic testing method based on deep learning |
| CN108256060B (en) * | 2018-01-16 | 2021-02-09 | 广州视源电子科技股份有限公司 | Closed loop detection method, device, terminal and storage medium |
-
2019
- 2019-03-06 CN CN201910168523.XA patent/CN109614459B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109614459A (en) | 2019-04-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109614459B (en) | Map structuring winding detection method and equipment applied to two-dimensional laser | |
| Cai et al. | Practical optimal registration of terrestrial LiDAR scan pairs | |
| CN110866496B (en) | Robot positioning and mapping method and device based on depth image | |
| US20210190497A1 (en) | Simultaneous location and mapping (slam) using dual event cameras | |
| KR102125959B1 (en) | Method and apparatus for determining a matching relationship between point cloud data | |
| CN113432600B (en) | Robot instant positioning and map construction method and system based on multiple information sources | |
| CN108594816B (en) | Method and system for realizing positioning and composition by improving ORB-SLAM algorithm | |
| CN105760811B (en) | Global map closed loop matching process and device | |
| KR102543952B1 (en) | Lane line determination method and apparatus, lane line positioning accuracy evaluation method and apparatus, device, and program | |
| Huang et al. | On the complexity and consistency of UKF-based SLAM | |
| US20110299787A1 (en) | Invariant visual scene and object recognition | |
| WO2022179094A1 (en) | Vehicle-mounted lidar external parameter joint calibration method and system, medium and device | |
| CN111127584A (en) | Method and device for establishing visual map, electronic equipment and storage medium | |
| Xiao et al. | Accurate extrinsic calibration between monocular camera and sparse 3D lidar points without markers | |
| US8126275B2 (en) | Interest point detection | |
| CN109489660A (en) | Robot localization method and apparatus | |
| Mayer | Efficient hierarchical triplet merging for camera pose estimation | |
| Zhang et al. | Optimally observable and minimal cardinality monocular SLAM | |
| Daudt et al. | Learning to understand earth observation images with weak and unreliable ground truth | |
| CN114528453A (en) | Global repositioning method and device, electronic equipment and storage medium | |
| CN115147561A (en) | Pose graph generation method, high-precision map generation method and device | |
| Lim et al. | Online 3D reconstruction and 6-DoF pose estimation for RGB-D sensors | |
| CN105787493A (en) | BIM-based method for intelligent extraction of setting-out feature points | |
| CN111220967A (en) | Method and device for detecting data validity of laser radar | |
| Li et al. | UniMODE: Unified Monocular 3D Object Detection |
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 |