CN107144292B - A kind of the odometer method and mileage counter device of sports equipment - Google Patents
A kind of the odometer method and mileage counter device of sports equipment Download PDFInfo
- Publication number
- CN107144292B CN107144292B CN201710426765.5A CN201710426765A CN107144292B CN 107144292 B CN107144292 B CN 107144292B CN 201710426765 A CN201710426765 A CN 201710426765A CN 107144292 B CN107144292 B CN 107144292B
- Authority
- CN
- China
- Prior art keywords
- pose
- laser
- current time
- laser frame
- map
- 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
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
The invention discloses a kind of odometer method of sports equipment and mileage counter devices, pose, the second laser frame at current time and partial points cloud map including obtaining sports equipment current time；According to the pose at current time, and second laser frame and partial points cloud map, the optimal pose at current time is calculated, odometer method provided by the invention, initial relative movement amount and pose are obtained by code-disc revolution, laser frame matching is carried out by iteration nearest neighbor algorithm etc., accurate amount of relative motion and pose are obtained with this.The pose obtained using previous step matches laser frame with partial points cloud map, obtains more accurate pose and amount of exercise, and externally export as initial value.Mileage counter device provided by the invention, solves that existing existing odometer is inaccurate and the deficiency of cumulative errors, can be used in numerous general or special purpose computing system environments or configuration.
Description
Technical field
The invention belongs to the Motion Technology fields of sports equipment, more particularly to the odometer method and mileage of sports equipment
Counter device.
Background technique
When mobile robot moves in environment indoors, odometer information is the most basic information of robot motion's state.
A kind of method that the odometer of robot is the mobile data using actuator to estimate the robot pose amount of changing over time.This
A pose variation can be obtained by encoder and inertial navigation sensors.Traditional encoder estimation course method is just with interior
Portion's sensor information is accumulated by mileage information, for example, a robot at its wheel or leg joint equipped with rotary coding
The equipment such as device, after it moves forward a period of time, it is desirable to know rough moving distance, by rotary encoder, can survey
The circle number of wheel rotation is measured, if it is known that the perimeter of wheel, can calculate the mobile distance of robot.
But this kind of odometer can always encounter precision problem.Since error is to mix generation, and error by many factors
The reading that accumulation at any time results in odometer becomes more and more unreliable as time increases.
Summary of the invention
In order to solve the defect that the low error of odometer precision is high in the prior art, the present invention provides a kind of sports equipments
Odometer method and mileage counter device.
The present invention is achieved through the following technical solutions: obtaining pose, the current time at sports equipment current time
Second laser frame and partial points cloud map；
According to the pose at current time and second laser frame and partial points cloud map, current time is calculated most
Excellent pose.
Further, using the pose at sports equipment current time as initial value, to second laser frame and partial points cloud map into
Row point cloud matching obtains optimal pose, i.e., the described optimal pose is the optimum solution obtained according to following formula, the formula are as follows:
Wherein:The curved surface constituted for partial points cloud map；piFor the laser point in second laser frame；SymbolGeneration
The transformation of table roto-translatory；SymbolRepresent the curved surface that point is constituted to partial points cloud mapEuclid
Projection；P is the pose in calculating process, using the pose at current time as initial value.
Further, the second laser frame is the second laser frame rejected after dynamic laser point.
Further, the step of rejecting the dynamic laser point in second laser frame includes: to be obtained according to the pose at current time
Position coordinates of the laser point in local probability grating map in second laser frame；Second laser is determined according to the position coordinates
Grid coordinate of the laser point on local probability grating map in frame；Determine that this swashs according to the probability value of grid where grid coordinate
Whether luminous point is dynamic laser point, if it is rejects dynamic laser point.
Further, the pose at the current time is more excellent pose, specifically, the acquisition of the more excellent pose includes step
It is rapid: to obtain the initial relative movement amount at sports equipment the first moment to current time；Obtain the first laser frame at the first moment with
And the second laser frame at current time；According to initial relative movement amount and first laser frame and second laser frame, interframe is obtained
Amount of relative motion；It is calculated according to the pose of the first moment sports equipment and interframe amount of relative motion and obtains the more excellent of current time
Pose.
Further, using amount of relative motion as initial value, frame matching is carried out with second laser frame to first laser frame and is obtained
Interframe movement amount.
Further, the interframe movement amount is obtained especially by the optimum solution for solving following formula, formula are as follows:
Wherein:The curved surface constituted for first laser frame；piFor the laser point of second laser frame；Q is in calculating process
The amount of relative motion of sports equipment, using amount of relative motion as initial value；SymbolRepresent roto-translatory transformation；SymbolRepresent the curved surface that point is constituted to first laser frameEuclid projection.
Further, map is updated according to optimal pose and second laser frame, including updates partial points cloud map and/or more
New local probability grating map.
Further, when the displacement of amount of relative motion reaches given threshold, resetting grating map, the resetting grid
Figure includes the probability value of resetting grating map, and resetting map origin updates local probability grating map.
The present invention also provides a kind of mileage counter device, technical solution is that the mileage counter device includes
First obtains module, obtains the pose at sports equipment current time；
Second obtains module, obtains the second laser frame at sports equipment current time；
Third obtains module, obtains partial points cloud map；
First computing module is calculated according to the pose at current time and second laser frame and partial points cloud map
The optimal pose at current time；
Further, described device further includes rejecting module, for rejecting the dynamic laser point in second laser frame；It is described
Second, which obtains module, obtains the second laser frame after rejecting dynamic laser point.
Further, the rejecting module includes
First coordinate unit, in local probability grid according to laser point in the pose at current time acquisition second laser frame
The position coordinates of figure；
Second coordinate unit determines that laser point is in local probability grating map in second laser frame according to the position coordinates
On grid coordinate；
Judging unit judges whether the laser point is dynamic laser point according to the probability value of grid where grid coordinate；
Culling unit, the judging result of judging unit reject dynamic laser point, i.e. judging result is that dynamic laser point then picks
It removes.
Further, described device further includes map rejuvenation module, for being updated according to optimal pose and second laser frame
Partial points cloud map and/or update local probability grating map.
It further, further include map resetting module, for resetting local probability grid according to the moving distance of sports equipment
Lattice map.
Further, described first the more excellent pose that module is specifically used for acquisition current time is obtained, including
First acquisition unit obtains the initial relative movement amount at sports equipment the first moment to current time；
Second acquisition unit obtains the first laser frame and the second laser frame at current time at the first moment；
First computing unit obtains interframe phase according to initial relative movement amount and first laser frame and second laser frame
To amount of exercise；
Second computing unit is calculated according to the pose of the first moment sports equipment and interframe amount of relative motion and is obtained currently
The more excellent pose at moment；
The present invention also provides another mileage counter devices, including processor and the instruction that can be performed for storage processor
Memory；
The processor is configured to:
Pose, the second laser frame at current time and the partial points cloud map at sports equipment current time are obtained,
According to the pose at current time and second laser frame and partial points cloud map, the optimal position at current time is obtained
Appearance.Beneficial effects of the present invention are as follows:
Odometer method provided by the invention obtains initial relative movement amount and pose by code-disc revolution, passes through iteration
Nearest neighbor algorithm etc. carries out laser frame matching, obtains accurate amount of relative motion and pose with this.The position obtained with previous step
Appearance is initial value, matches to laser frame with partial points cloud map, obtains more accurate pose and amount of exercise, and externally export.
This method is not necessarily to priori global map, can effectively inhibit the accumulated error of odometer, to well adapting to property of dynamic environment；Greatly
Range closed loop (200 meters) error is within 1 meter.
Substantially the part that contributes to existing technology can be with software product in other words for technical solution of the present invention
Form embodies, which can store in storage medium, such as ROM/RAM, magnetic disk, CD, including
Some instructions are used so that a computer equipment (can be personal computer, server or the network equipment etc.).
Mileage counter device provided by the invention, solve that existing existing odometer is inaccurate and cumulative errors not
Foot can be used in numerous general or special purpose computing system environments or configuration.Such as: personal computer, server computer, hand
Holding equipment or portable device, laptop device, multicomputer system, microprocessor-based system, it is set-top box, programmable
Consumer-elcetronics devices, network PC, minicomputer, mainframe computer, the distributed computing including any of the above system or equipment
Environment etc..
Detailed description of the invention
Fig. 1 is the flow diagram of odometer embodiment of the method 1 of the present invention；
Fig. 2 is the flow diagram that odometer embodiment of the method 1 of the present invention obtains more excellent current pose；
Fig. 3 is the flow diagram of odometer embodiment of the method 2 of the present invention；
Fig. 4 is the module composition schematic diagram of mileage counter device of the present invention.
Specific embodiment
In conjunction with attached drawing and specific embodiment, the present invention is described further, it should be noted that do not colliding
Under the premise of, new embodiment can be formed in any combination between various embodiments described below or between each technical characteristic:
Embodiment one
Fig. 1 is the flow diagram of odometer method provided in this embodiment.
Equipment in the scene of this method application includes sports equipment, arithmetic facility, and is arranged on sports equipment
Laser Scanning Equipment and physics mileage equipment.Sports equipment can be mobile robot, sweeping robot, intelligent robotic toy or need
The smart home etc. to move and position indoors.For arithmetic facility for calculating various data, data source is movement
Equipment, Laser Scanning Equipment, physics mileage equipment etc..The exercise data that physics mileage equipment is used to detect acquisition sports equipment comes
It estimates the sports equipment pose amount of changing over time, including position change amount, angulation change amount etc., is generally positioned at sports equipment
On, such as code-disc, other photoelectric encoders or other can measure the detection of the displacement of sports equipment whithin a period of time
Element.
Certain zone of action generally is set with to sports equipment such as mobile robot.According to different sports equipments can
To establish different motion models, the motion model of differential driving, the motion model of Omni-mobile common are.Specific real
Applying has two adjacent moment in example, i.e. previous moment and current time, generally according to different motion model and known
Sports equipment previous moment pose (xt-1, yt-1, θt-1), the pose (x at sports equipment current time can be obtainedt, yt,
θt), and amount of relative motion (Δ x can be calculated according to the pose at two momentt, Δ yt, Δ θt).Such as two-wheel
The motion model of difference, the driving motor of left and right two-wheeled are mounted on code-disc as physics mileage equipment, can pass through following formula
Acquire the pose (x for carrying out current timet, yt, θt)
ΔDt=(Δ dL+ΔdR)/2
Δθt=(Δ dL-ΔdR)/ω
θt=θt-1+Δθt
Wherein, the spacing between left and right two-wheeled is ω, Δ DtTo calculate the sports equipment obtained according to the information of code-disc
Distance change amount, Δ θtFor the angle variable quantity of sports equipment, Δ dL、ΔdRThe left and right two respectively examined and seized by left and right codes
The amount of exercise of wheel；
For another example the driving motor of four driving wheels of the motion model of Omni-mobile is mounted on code-disc, can be by following public
Formula acquires the pose (x for carrying out current timet, yt, θt)
xt=xt-1+Δx
yt=yt-1+Δy
θt=θt-1+Δθ
Wherein, Δ d1, Δ d2, Δ d3, Δ d4Respectively by the amount of exercise on four driving wheels of code-disc acquisition, r is wheel
Hub radius, rrFor roller radius, wheel base 2lx, left and right wheels are away from for 2ly；
The scanning direction of Laser Scanning Equipment is the front of sports equipment moving direction, and Laser Scanning Equipment scanning obtains
Each laser frame data are presented in the form of laser point cloud the prior art.Set up global coordinate system, the pose of sports equipment,
The position of Laser Scanning Equipment and the laser frame data of acquisition can be unified into global coordinate system.
Partial points cloud map is formed by scanning local environment with Laser Scanning Equipment, is stored in arithmetic facility.Part
Probabilistic Cell map is then that partial points cloud map is divided into a series of grids, establishes local probability grating map coordinate system, wherein
Each grid gives a probable value, indicates the probability that the grid is occupied.Point cloud map coordinates system, grating map coordinate system with
Global coordinate system can be converted mutually, to determine pose of the sports equipment in map coordinates system.
The setting up of coordinate system, the point in coordinate transform and coordinate system is mapped to other coordinate systems can be by existing skill
Art is realized.
Odometer method as shown in Figure 1, comprising the following steps:
Step S110: pose, the second laser frame and partial points cloud at current time at sports equipment current time are obtained
Map.
The current time is t, and the pose at the current time can be obtained by physics mileage equipment or the prior art
Obtain the initial pose at current time
Simultaneously in current time t, the second laser frame scanned by Laser Scanning Equipment, Laser Scanning Equipment will be obtained
The laser frame data obtained are sent to arithmetic facility, can also specifically be set by master control by wired or wireless mode
It is standby that laser frame data are transmitted to arithmetic facility.
Step S120: according to the pose at current time and second laser frame and partial points cloud map, current time is obtained
Optimal pose.
Specifically, the partial points cloud map is stored in arithmetic facility, and arithmetic facility is first with the pose at current time
Value carries out point cloud matching to second laser frame and partial points cloud map and obtains optimal pose, i.e., obtains according to the following formula most
Optimization solution is optimal poseThe formula are as follows:
Wherein:The curved surface constituted for partial points cloud map；piFor the laser point in second laser frame；SymbolGeneration
The transformation of table roto-translatory；SymbolRepresent the curved surface that point is constituted to partial points cloud mapEuclid
Projection；P is the pose in calculating process, and using the pose at current time as initial value, final value is optimal pose
The second laser frame, which carries out point cloud matching with partial points cloud map, to be achieved by the prior art, such as iteration
Nearest neighbor algorithm, violence matching etc..The optimal pose be calculated through the above technical solution by arithmetic facility acquisition it is current when
It is more accurate directly to pass through the pose that the prior art or physics mileage equipment obtain than step S110 for the pose at quarter.
In the present embodiment, as a further improvement, the pose at the current time can also be through following technical side
The more excellent pose that case obtainsFor replacing can get the initial of current time by physics mileage equipment or the prior art
PoseFig. 2 is the flow diagram that odometer embodiment of the method 1 of the present invention obtains more excellent current pose；Specifically, described
The acquisition of more excellent pose the following steps are included:
Step S111: the initial relative movement amount at sports equipment the first moment to current time is obtained.
First moment is the sometime point before current time, generally the previous moment t-1 at current time.Institute
Initial relative movement amount is stated for describing sports equipment pose P locating for the first moment t-1t-1=(xt-1, yt-1, θt-1) mobile
To the variation of pose locating for current time t, location variation and angle variable quantity including sports equipment are usedIt indicates.
Wherein, pose P locating for the first moment t-1t-1=(xt-1, yt-1, θt-1) known, it is preferable that first moment
Pose P locating for t-1t-1=(xt-1, yt-1, θt-1) it is the optimal pose that last odometer method obtains.In a specific implementation
In, it can the pose P according to locating for the first moment t-1t-1=(xt-1, yt-1, θt-1) with current time t locating for initial poseCalculating is converted into initial relative movement amountCalculation method is existing skill
Art, and have been described in front.
It is sent to arithmetic facility by the data that physics mileage equipment or other prior arts obtain, passes through wired or nothing
The mode of line can also be transmitted to arithmetic facility by total control equipment and be calculated.
Step S112: the first laser frame and the second laser frame at current time at the first moment are obtained.
In the first moment t-1, the laser frame that Laser Scanning Equipment scans is first laser frame, in current time t, is swashed
The laser frame that optical scanning device scans is second laser frame.
Step S113: according to initial relative movement amount and first laser frame and second laser frame, the opposite fortune of interframe is obtained
Momentum.
Specifically, using the initial relative movement amount obtained in step S111 as initial value, to first laser frame and second laser
Frame carries out frame matching and obtains interframe amount of relative motion, i.e., the optimum solution obtained according to the following formula is interframe movement amount,
The formula are as follows:
Wherein:The curved surface constituted for first laser frame；piFor the laser point of second laser frame；Q is in calculating process
The amount of relative motion of sports equipment, using amount of relative motion as initial value, final value is interframe amount of relative motion；Symbol
Represent roto-translatory transformation；SymbolRepresent the curved surface that point is constituted to first laser frameEuclid throw
Shadow.
The frame matching of the first laser frame and second laser frame can be achieved by the prior art, such as iteration neighbour
Algorithm, violence matching etc..
The interframe amount of relative motion be calculated on the basis of initial relative movement amount by arithmetic facility obtain from the
Pose locating for one moment t-1 is moved to the variable quantity of pose locating for current time t, first relative to what is obtained in step S111
Beginning amount of relative motion is more acurrate.
Step S114: it is calculated according to the pose of the first moment sports equipment and interframe amount of relative motion and obtains current time
More excellent pose.
The pose of first moment sports equipment is it is known that according to the pose P of the first moment t-1 sports equipmentt-1=(xt-1,
yt-1, θt-1) and interframe amount of relative motion can calculate the more excellent pose for being converted into current timeThe more excellent pose
It is more accurate relative to the pose for directly passing through the current time that physics mileage equipment or other prior arts obtain.
Further, the invention also includes step S131: according to the pose and optimal pose of the first moment sports equipment
Calculating is converted into optimal amount of relative motion.
The pose of first moment sports equipment is it is known that according to the pose P of the first moment t-1 sports equipmentt-1=(xt-1,
yt-1, θt-1) and optimal poseIt can calculate and obtain optimal amount of relative motion
It, can be according to the pose P of the first moment t-1 in step S111 in the present inventiont-1=(xt-1, yt-1, θt-1) and it is current
The pose of moment tCalculating be converted into initial relative movement amount, calculation formula is as follows:
xt=xt-1+cos(θt-1)Δxt-sin(θt-1)Δyt
yt=yt-1+sin(θt-1)Δxt+cos(θt-1)Δyt
θt=θt-1+Δθt
Wherein, with the more excellent pose at current timeInstead of the initial pose of current time tIt can be achieved in step S114
The calculating of more excellent pose and interframe amount of relative motion converts；With optimal poseInstead of the pose of current time tStep can be achieved
Optimal pose and optimal amount of relative motion, which calculate, in rapid S131 converts.Meanwhile it through the invention can be with above-mentioned formula according to fortune
The pose and optimal pose at dynamic first moment of equipment, which calculate, obtains optimal amount of relative motion.
Odometer method provided by the invention obtains initial relative movement amount and pose by code-disc revolution, passes through iteration
Nearest neighbor algorithm etc. carries out laser frame matching, obtains accurate amount of relative motion and pose with this.The position obtained with previous step
Appearance is initial value, matches to laser frame with partial points cloud map, obtains more accurate pose and amount of exercise, and externally export.
This method is not necessarily to priori global map, can effectively inhibit the accumulated error of odometer, to well adapting to property of dynamic environment；Greatly
Range closed loop (200 meters) error is within 1 meter.
Embodiment two
Fig. 3 is the flow diagram of odometer embodiment of the method 2 of the present invention.
The odometer method of the present embodiment includes
Step S210: second after obtaining the pose at sports equipment current time, the rejecting dynamic Laser point at current time
Laser frame and partial points cloud map.
Local probability grating map is introduced, using the probability value of Probabilistic Cell map as the foundation for deleting dynamic point.By
Dual-laser frame is mapped to local probability grating map, if the probability value of grid where laser point is less than the minimum probability of static grid
Value is then dynamic grid, i.e. laser point in the grid is dynamic laser point, specifically, comprising the following steps:
Step S211: according to laser point in the pose at current time acquisition second laser frame in local probability grating map
Position coordinates；
Step S212: determine that laser point is on local probability grating map in second laser frame according to the position coordinates
Grid coordinate；
Step S213: determine whether the laser point is dynamic laser point according to the probability value of grid where grid coordinate, such as
Fruit is to reject dynamic laser point.
Principle is as follows:
Laser point is laser point i in second laser frame, and second laser frame is mapped to part generally by roto-translatory transformation
Rate grating map obtains the position coordinates L of laser point ii, i.e.,
The origin O of local probability grating map, grid size reso, grid where laser point i grid coordinate (m,
N), then
Then as 0≤Bel (m, n)≤BelthresWhen, determine that laser point i is dynamic laser point in the grid.Wherein, Bel (m,
It n) is the probability value of grid, BelthresFor the minimum probability value of static grid, Bel (m, n), BelthresValue, dynamic laser point
Rejecting be achieved by the prior art, the present invention repeats no more.
Step S220: according to the pose at current time, and second laser frame and partial points after dynamic laser point are rejected
Cloud map obtains the optimal pose at current time.
Specifically, using the pose at current time as initial value, with partial points cloud to the second laser frame after rejecting dynamic point
Figure carries out point cloud matching and obtains optimal pose, i.e., the optimum solution obtained according to the following formula is optimal poseThe public affairs
Formula are as follows:
Wherein:The curved surface constituted for partial points cloud map；To reject the second laser frame after dynamic laser point
Interior laser point；P is the pose in calculating process, and using the pose at current time as initial value, final value is optimal pose；
SymbolRepresent roto-translatory transformation；SymbolRepresent the curved surface that point is constituted to partial points cloud map
Euclid projection.
Odometer method described in the present embodiment unlike embodiment one in step S210 and step S220 the
Dual-laser frame is the laser frame rejected after dynamic laser point, step S210, the current time in step S220 and step S211
The more excellent pose at the current time that pose can also be obtained with the method by step S111- step S114.
In another embodiment, odometer method of the invention further includes step S240: being obtained most according to step S220
Excellent pose and second laser frame update map, including update local probability grating map and update partial points cloud map.This step
The process that the local probability grating map and partial points cloud map application of acquisition are realized in odometer method next time, to obtain more
Accurate mileage specifically includes following sub-step:
Step S241: the laser point in second laser frame is mapped in global coordinate system and obtains laser point in world coordinates
The position of system.Wherein, laser point i can be to reject the laser point in the second laser frame after dynamic laser point, and laser point i is complete
The position of office's coordinate systemIt is converted by roto-translatory and second laser frame is mapped in global coordinate system, i.e.,
Step S242: the position is inserted into partial points cloud map, completes the update of partial points cloud map；This step can
To be achieved by the prior art, which is not described herein again；
Step S243: in a manner of ray tracing, local probability grating map is updated；Including updating the grid near location point
Probability value and Laser Scanning Equipment and location point line on probability value.Specifically, generally with the lesser Gauss of variance point
Cloth updates location pointThe probability value of neighbouring grid；By Laser Scanning Equipment and location pointGrid probability on line is set
Zero, zero indicates that the probability that the grid is occupied is 0.
In another embodiment, the continuous accumulated distance of odometer further includes step as another improvement of the invention
S250 resets Probabilistic Cell map according to the moving distance of sports equipment.Specifically, including sub-step:
Step S251: calculating the moving distance of sports equipment, and judges whether to reach set distance；If reaching set distance
It then carries out step S252, step S253, step S254 and carries out resetting map, the resetting grating map includes resetting grating map
Probability value, resetting map origin, update local probability grating map.The moving distance of the sports equipment is through the invention
The odometer that odometer method is realized obtains.
All grid probability values: being set to the probability value for indicating unknown by step S252, and general -1 indicates the general of the grid
Rate value is unknown, i.e., the grid does not know whether to be occupied；
Step S253: according to position of the optimal pose in map definitely primitive point；Specifically, with optimal pose on ground
The position of position or translation certain distance in figure is map origin；
Step S254: local probability grating map is updated in a manner of ray tracing with current time nearest N frame laser frame.
Update method is consistent with step S243, can obtain the position of more accurate sports equipment in odometer method next time with this
Appearance.
Embodiment three
Fig. 4 is the module composition schematic diagram of mileage counter device of the present invention.
A kind of mileage counter device, including
First obtains module, obtains the pose at sports equipment current time；
Second obtains module, obtains the second laser frame at sports equipment current time；
Third obtains module, obtains partial points cloud map；
First computing module is calculated according to the pose at current time and second laser frame and partial points cloud map
The optimal pose at current time.
In the present embodiment, as a further improvement, it is described first obtain module be specifically used for obtain current time compared with
Excellent pose pose, specifically, the first acquisition module includes
First acquisition unit obtains the initial relative movement amount at sports equipment the first moment to current time.
Second acquisition unit obtains the first laser frame and the second laser frame at current time at the first moment.
First computing unit obtains interframe phase according to initial relative movement amount and first laser frame and second laser frame
To amount of exercise.
Second computing unit is calculated according to the pose of the first moment sports equipment and interframe amount of relative motion and is obtained currently
The more excellent pose at moment.
Further, mileage counter device of the invention further includes rejecting module, for rejecting the dynamic in second laser frame
Laser point, described second, which obtains module, obtains the second laser frame after rejecting dynamic laser point.Specifically, the rejecting module packet
It includes:
First coordinate unit, in local probability grid according to laser point in the pose at current time acquisition second laser frame
The position coordinates of figure；
Second coordinate unit determines that laser point is in local probability grating map in second laser frame according to the position coordinates
On grid coordinate；
Judging unit judges whether the laser point is dynamic laser point according to the probability value of grid where grid coordinate,
Culling unit, the judging result of judging unit reject dynamic laser point, i.e. judging result is that dynamic laser point then picks
It removes.
Further, mileage counter device of the invention further includes
For updating partial points cloud map and local probability grating map according to optimal pose and second laser frame.Specifically
Ground, the map rejuvenation module include:
Laser point in second laser frame is mapped in global coordinate system by map unit, is obtained laser point and is sat in the overall situation
Mark the position of system.It is converted by roto-translatory, i.e., it will by formulaLaser in second laser frame
Point is mapped in global coordinate system.
Position is inserted into partial points cloud map, the update of partial points cloud map is completed with this by the first updating unit.
Second updating unit updates local probability grating map in a manner of ray tracing.Specifically, generally with variance compared with
Small Gaussian Profile updates location pointThe probability value of neighbouring grid；By Laser Scanning Equipment and location pointOn line
Grid probability zero setting, zero indicates the probability value that the grid point is not occupied.
Further, mileage counter device of the invention further includes map resetting module, for the movement according to sports equipment
Distance resetting local probability grating map.The map resets module
Metrics calculation unit calculates the moving distance of sports equipment；The calculation method is the prior art, or passes through this
The odometer of method is calculated according to optimal pose and is obtained.
Second judgment unit, judges whether the moving distance reaches set distance；
If reaching set distance, the map resetting module passes through probability value reset cell, origin reset cell, map weight
Set unit resetting local probability grating map.That is probability value reset cell sets all grid probability values according to judging result
To indicate unknown probability value；Position reset of the optimal pose in map is ground according to judging result by origin reset cell
Primitive point；Map reset cell, according to judging result, in a manner of ray tracing, more using current time nearest N frame laser frame
New local probability grating map.
The method in device and previous embodiment in the present embodiment be based on the same inventive concept under two aspect,
Front is described in detail method implementation process, so those skilled in the art can be according to foregoing description clearly
The structure and implementation process of the system in this implementation are solved, in order to illustrate the succinct of book, details are not described herein again.
For convenience of description, it is divided into various modules when description apparatus above with function to describe respectively.Certainly, implementing this
The function of each module can be realized in the same or multiple software and or hardware when invention.
As seen through the above description of the embodiments, those skilled in the art can be understood that the present invention can
It realizes by means of software and necessary general hardware platform.Based on this understanding, technical solution of the present invention essence
On in other words the part that contributes to existing technology can be embodied in the form of software products, the computer software product
It can store in storage medium, such as ROM/RAM, magnetic disk, CD, including some instructions are used so that a computer equipment
(can be personal computer, server or the network equipment etc.) executes the certain of each embodiment or embodiment of the invention
Method described in part.
The Installation practice of description is only schematical, wherein described, module or unit can as illustrated by the separation member
It is physically separated with being or may not be, the component illustrated as module or unit may or may not be object
Module is managed, both can be located in one place, or may be distributed on multiple network modules.It can select according to the actual needs
Some or all of unit therein is selected to achieve the purpose of the solution of this embodiment.Those of ordinary skill in the art are not paying wound
In the case that the property made is worked, it can understand and implement.
The present invention can be used in numerous general or special purpose computing system environments or configuration.Such as: personal computer, service
Device computer, handheld device or portable device, laptop device, multicomputer system, microprocessor-based system, machine top
Box, programmable consumer-elcetronics devices, network PC, minicomputer, mainframe computer, including any of the above system or equipment
Distributed computing environment etc., such as example IV.
Example IV
Mileage counter device as shown in the figure, comprising: the storage of processor and the instruction that can be performed for storage processor
Device；
The processor is configured to:
Pose, the second laser frame at current time and the partial points cloud map at sports equipment current time are obtained,
According to the pose at current time and second laser frame and partial points cloud map, the optimal position at current time is obtained
Appearance.
Odometer provided in an embodiment of the present invention, solve that existing existing odometer is inaccurate and cumulative errors not
Foot.For those skilled in the art, it can make various other corresponding according to the above description of the technical scheme and ideas
Change and deformation, and all these change and deformation all should belong to the claims in the present invention protection scope it
It is interior.
Claims (15)
1. a kind of odometer method, which is characterized in that including
Obtain pose, the second laser frame at current time and the partial points cloud map at sports equipment current time；According to current
The pose and second laser frame and partial points cloud map at moment, are calculated the optimal pose at current time, with sports equipment
The pose at current time is initial value, carries out point cloud matching to second laser frame and partial points cloud map and obtains optimal pose, described
Optimal pose is obtained especially by the optimum solution for solving following formula, the formula are as follows:
Wherein:The curved surface constituted for partial points cloud map；piFor the laser point in second laser frame；SymbolRepresent rotation
Turn-translation transformation；SymbolRepresent the curved surface that point is constituted to partial points cloud mapEuclid projection；
P is the pose in calculating process, using the pose at current time as initial value.
2. odometer method according to claim 1, which is characterized in that the second laser frame is to reject dynamic laser point
Second laser frame afterwards.
3. odometer method according to claim 2, which is characterized in that reject the dynamic laser point in second laser frame
Step includes:
According to the pose at current time obtain second laser frame in laser point local probability grating map position coordinates；
Grid coordinate of the laser point on local probability grating map in second laser frame is determined according to the position coordinates；
It determines whether the laser point is dynamic laser point according to the probability value of grid where grid coordinate, if it is rejects dynamic
Laser point.
4. odometer method according to claim 1, which is characterized in that the pose at the current time is more excellent pose,
Specifically, the acquisition of the more excellent pose comprising steps of
Obtain the initial relative movement amount at sports equipment the first moment to current time；
Obtain the first laser frame at the first moment and the second laser frame at current time；
According to initial relative movement amount and first laser frame and second laser frame, interframe amount of relative motion is obtained；
The more excellent pose for obtaining current time is calculated according to the pose of the first moment sports equipment and interframe amount of relative motion.
5. odometer method according to claim 4, which is characterized in that using amount of relative motion as initial value, to first laser
Frame and second laser frame carry out frame matching and obtain interframe amount of relative motion.
6. odometer method according to claim 5, which is characterized in that the interframe amount of relative motion is especially by solution
The optimum solution of following formula obtains, the formula are as follows:
Wherein:The curved surface constituted for first laser frame；piFor the laser point of second laser frame；Q is to move to set in calculating process
Standby amount of relative motion, using amount of relative motion as initial value；SymbolRepresent roto-translatory transformation；SymbolIt represents
The curved surface that point is constituted to first laser frameEuclid projection.
7. odometer method according to claim 1-6, which is characterized in that according to optimal pose and second laser
Frame updates map, including updates partial points cloud map and/or update local probability grating map.
8. odometer method according to claim 1-6, which is characterized in that when the displacement of amount of relative motion reaches
When given threshold, grating map is reset, the resetting grating map includes the probability value for resetting grating map, resetting ground primitive
Point updates local probability grating map.
9. a kind of for implementing the mileage counter device of odometer method described in any one of claim 1-6, feature exists
In, including
First obtains module, obtains the pose at sports equipment current time；
Second obtains module, obtains the second laser frame at sports equipment current time；
Third obtains module, obtains partial points cloud map；
First computing module is calculated current according to the pose at current time and second laser frame and partial points cloud map
The optimal pose at moment.
10. mileage counter device according to claim 9, which is characterized in that described device further includes rejecting module, for picking
Except the dynamic laser point in second laser frame；Described second, which obtains module, obtains the second laser frame after rejecting dynamic laser point.
11. mileage counter device according to claim 10, which is characterized in that the rejecting module includes
First coordinate unit, according to laser point in the pose at current time acquisition second laser frame in local probability grating map
Position coordinates；
Second coordinate unit determines that laser point is on local probability grating map in second laser frame according to the position coordinates
Grid coordinate；
Judging unit judges whether the laser point is dynamic laser point according to the probability value of grid where grid coordinate；
Culling unit, the judging result of judging unit reject dynamic laser point, i.e. judging result is that dynamic laser point is then rejected.
12. mileage counter device according to claim 9, which is characterized in that described device further includes map rejuvenation module, is used
According to optimal pose and second laser frame update partial points cloud map and/or update local probability grating map.
13. mileage counter device according to claim 9, which is characterized in that further include map resetting module, for according to fortune
The moving distance of dynamic equipment resets local probability grating map.
14. mileage counter device according to claim 9, which is characterized in that described first, which obtains module, is specifically used for obtaining
The more excellent pose at current time, including
First acquisition unit obtains the initial relative movement amount at sports equipment the first moment to current time；
Second acquisition unit obtains the first laser frame and the second laser frame at current time at the first moment；
First computing unit obtains the opposite fortune of interframe according to initial relative movement amount and first laser frame and second laser frame
Momentum；
Second computing unit calculates according to the pose of the first moment sports equipment and interframe amount of relative motion and obtains current time
More excellent pose.
15. a kind of for implementing the mileage counter device of odometer method described in any one of claim 1-6, feature exists
In the mileage counter device includes: the memory of processor and the instruction that can be performed for storage processor；
The processor is configured to:
Pose, the second laser frame at current time and the partial points cloud map at sports equipment current time are obtained,
According to the pose at current time and second laser frame and partial points cloud map, the optimal pose at current time is obtained.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710426765.5A CN107144292B (en) | 2017-06-08 | 2017-06-08 | A kind of the odometer method and mileage counter device of sports equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710426765.5A CN107144292B (en) | 2017-06-08 | 2017-06-08 | A kind of the odometer method and mileage counter device of sports equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107144292A CN107144292A (en) | 2017-09-08 |
| CN107144292B true CN107144292B (en) | 2019-10-25 |
Family
ID=59779639
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710426765.5A Active CN107144292B (en) | 2017-06-08 | 2017-06-08 | A kind of the odometer method and mileage counter device of sports equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107144292B (en) |
Families Citing this family (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108253958B (en) * | 2018-01-18 | 2020-08-11 | 亿嘉和科技股份有限公司 | Robot real-time positioning method in sparse environment |
| CN108844553B (en) * | 2018-06-27 | 2021-06-15 | 广州视源电子科技股份有限公司 | Method and device for correcting mileage in robot moving process and robot |
| CN109631919B (en) * | 2018-12-28 | 2022-09-30 | 芜湖哈特机器人产业技术研究院有限公司 | Hybrid navigation map construction method integrating reflector and occupied grid |
| CN111609853B (en) * | 2019-02-25 | 2024-08-23 | 北京奇虎科技有限公司 | Three-dimensional map construction method, sweeping robot and electronic equipment |
| CN110824496B (en) * | 2019-09-18 | 2022-01-14 | 北京迈格威科技有限公司 | Motion estimation method, motion estimation device, computer equipment and storage medium |
| CN110887493B (en) * | 2019-11-29 | 2023-05-05 | 上海有个机器人有限公司 | Track calculation method, medium, terminal and device based on local map matching |
| CN110969649B (en) * | 2019-11-29 | 2024-04-05 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device for laser point cloud and map |
| US11725944B2 (en) | 2020-03-02 | 2023-08-15 | Apollo Intelligent Driving Technology (Beijing) Co, Ltd. | Method, apparatus, computing device and computer-readable storage medium for positioning |
| CN111735451B (en) * | 2020-04-16 | 2022-06-07 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
| CN111637877B (en) * | 2020-05-29 | 2022-06-21 | 拉扎斯网络科技（上海）有限公司 | Robot positioning method and device, electronic equipment and nonvolatile storage medium |
| CN111696159B (en) * | 2020-06-15 | 2021-06-11 | 湖北亿咖通科技有限公司 | Feature storage method of laser odometer, electronic device and storage medium |
| CN112344940B (en) * | 2020-11-06 | 2022-05-17 | 杭州国辰机器人科技有限公司 | Positioning method and device integrating reflective columns and grid map |
| CN112700495B (en) * | 2020-11-25 | 2024-08-16 | 北京旷视机器人技术有限公司 | Pose determination method, pose determination device, robot, electronic equipment and storage medium |
| CN113012191B (en) * | 2021-03-11 | 2022-09-02 | 中国科学技术大学 | Laser mileage calculation method based on point cloud multi-view projection graph |
| CN115235477A (en) * | 2021-11-30 | 2022-10-25 | 上海仙途智能科技有限公司 | Vehicle positioning inspection method and device, storage medium and equipment |
| CN115371678A (en) * | 2022-08-22 | 2022-11-22 | 广州足下科技有限公司 | Autonomous navigation method for mobile robot |
| CN117170354B (en) * | 2022-10-25 | 2024-08-09 | 中科南京软件技术研究院 | Wheel type robot positioning method and system |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106767827A (en) * | 2016-12-29 | 2017-05-31 | 浙江大学 | A kind of mobile robot point cloud map creating method based on laser data |
-
2017
- 2017-06-08 CN CN201710426765.5A patent/CN107144292B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106767827A (en) * | 2016-12-29 | 2017-05-31 | 浙江大学 | A kind of mobile robot point cloud map creating method based on laser data |
Non-Patent Citations (1)
| Title |
|---|
| 基于三维激光扫描仪的室内移动设备定位与建图;赵梓乔等;《计算机与数字工程》;20161130;第2252-2256页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107144292A (en) | 2017-09-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107144292B (en) | A kind of the odometer method and mileage counter device of sports equipment | |
| CN105247431B (en) | Autonomous body | |
| US11086016B2 (en) | Method and apparatus for tracking obstacle | |
| Philipp et al. | Mapgenie: Grammar-enhanced indoor map construction from crowd-sourced data | |
| CN102609942B (en) | Depth map is used to carry out mobile camera location | |
| CN102395192B (en) | Method and device for locating wireless sensor terminal | |
| CN113781582A (en) | Synchronous positioning and map creating method based on laser radar and inertial navigation combined calibration | |
| CN110196044A (en) | It is a kind of based on GPS closed loop detection Intelligent Mobile Robot build drawing method | |
| CN102573049A (en) | Method and system for indoor positioning | |
| Pan et al. | Voxfield: Non-projective signed distance fields for online planning and 3d reconstruction | |
| Fang et al. | A Real‐Time 3D Perception and Reconstruction System Based on a 2D Laser Scanner | |
| CN110375712A (en) | Drift section extracting method, device, equipment and storage medium | |
| AU2022259832A1 (en) | Target detection in a point cloud | |
| CN109302234B (en) | Method for calculating complex channel impulse response of indoor visible light communication system | |
| He et al. | Volumeter: 3D human body parameters measurement with a single Kinect | |
| Yi et al. | An Enhanced Hybrid 3D Localization Algorithm Based on APIT and DV-Hop. | |
| Hu et al. | PALoc: Advancing SLAM Benchmarking With Prior-Assisted 6-DoF Trajectory Generation and Uncertainty Estimation | |
| Mozamir et al. | Indoor localization estimation techniques in wireless sensor network: A review | |
| CN105806331A (en) | Positioning method for indoor robot and indoor robot | |
| Motroni et al. | Self-locating RFID robot for tag localization in retails | |
| CN106937298B (en) | A kind of improved wireless sensor network 3-D positioning method | |
| CN112045654B (en) | Detection method and device for unmanned closed space and robot | |
| Zhang et al. | A Lidar SLAM based on Improved Particle Filter and Scan Matching for Unmanned Delivery Robot | |
| Zhang et al. | Localizing tags using mobile infrastructure | |
| Brscic et al. | Model based robot localization using onboard and distributed laser range finders |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220324 Address after: Room 101-110, 1st floor, building 8, Jinsheng Industrial Park, 611 Dongguan Road, Binjiang District, Hangzhou, Zhejiang 310000 Patentee after: HANGZHOU IPLUS TECH CO.,LTD. Address before: 310000 229, room 1, No. 475 Changhe Road, Changhe street, Binjiang District, Hangzhou, Zhejiang Patentee before: HANGZHOU NANJIANG ROBOTICS Co.,Ltd. |