CN107144292A - The odometer method and mileage counter device of a kind of sports equipment - Google Patents
The odometer method and mileage counter device of a kind of sports equipment Download PDFInfo
- Publication number
- CN107144292A CN107144292A CN201710426765.5A CN201710426765A CN107144292A CN 107144292 A CN107144292 A CN 107144292A CN 201710426765 A CN201710426765 A CN 201710426765A CN 107144292 A CN107144292 A CN 107144292A
- Authority
- CN
- China
- Prior art keywords
- pose
- laser
- current time
- laser frame
- frame
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
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 device, including obtain pose, the second laser frame at current time and the partial points cloud map at sports equipment current time；According to the pose at current time, and second laser frame and partial points cloud map, calculating obtains the steps such as the optimal pose at current time, the odometer method that the present invention is provided, 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 is matched as initial value to laser frame with partial points cloud map, obtains more accurately pose and amount of exercise, and externally export.The mileage counter device that the present invention is provided, solves that existing existing odometer is inaccurate and deficiencies of cumulative errors, available in numerous general or special purpose computing system environments or configuration.
Description
Technical field
The invention belongs to the odometer method and mileage in the Motion Technology field of sports equipment, more particularly to sports equipment
Counter device.
Background technology
When mobile robot is moved 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 mobile data using actuator to estimate the robot pose amount of changing over time.This
Individual pose change can be obtained by encoder and inertial navigation sensors.Traditional encoder estimates course method just with interior
Portion's sensor information is accumulated by mileage information, and such as one robot is 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 displacement substantially, by rotary encoder, can survey
The number of turns of wheel rotation is measured, if it is known that the girth of wheel, just can calculate the distance of robot movement.
But this kind of odometer can always run into precision problem.Because error is to be mixed to produce by many factors, and error
The reading that result in odometer with the accumulation of time increases over time and becomes more and more unreliable.
The content of the invention
In order to solve the high defect of the low error of odometer precision in the prior art, the invention provides a kind of sports equipment
Odometer method and mileage counter device.
The present invention is achieved through the following technical solutions：Obtain 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, calculating obtains current time most
Excellent pose.
Further, using the pose at sports equipment current time as initial value, second laser frame is entered with partial points cloud map
Row point cloud matching obtains optimal pose, i.e., described optimal pose is the optimum solution obtained according to below equation, and the formula is：
Wherein：The curved surface constituted for partial points cloud map；piFor the laser spots of second laser frame in；SymbolGeneration
Table roto-translatory is converted；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 after rejecting dynamic laser point.
Further, the step of rejecting the dynamic laser point in second laser frame includes：Obtained according to the pose at current time
Obtain position coordinates of the second laser frame in laser spots in local probability grating map；Second laser is determined according to the position coordinates
Grid coordinate of the frame in laser spots on local probability grating map；The probable value of grid according to where grid coordinate determines that this swashs
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
Suddenly：Obtain the initial relative movement amount at the moment of sports equipment first 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；Calculated according to the pose of the first moment sports equipment and interframe amount of relative motion and obtain the more excellent of current time
Pose.
Further, using amount of relative motion as initial value, frame matching is carried out to first laser frame and second laser frame and obtained
Interframe movement amount.
Further, the interframe movement amount is obtained especially by the optimum solution for solving below equation, and formula is：
Wherein：The curved surface constituted for first laser frame；piFor the laser spots of second laser frame；Q is fortune in calculating process
The amount of relative motion of dynamic equipment, using amount of relative motion as initial value；SymbolRepresent roto-translatory conversion；Symbol
Represent 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, replacement grating map, the replacement grid
Figure includes resetting the probable value of grating map, resets map origin, updates local probability grating map.
The present invention also provides a kind of mileage counter device, and its technical scheme is that the mileage counter device includes
First acquisition module, obtains the pose at sports equipment current time；
Second acquisition module, obtains the second laser frame at sports equipment current time；
3rd acquisition module, obtains partial points cloud map；
First computing module, according to the pose at current time, and second laser frame and partial points cloud map, calculating is obtained
The optimal pose at current time；
Further, described device also includes rejecting module, the dynamic laser point for rejecting second laser frame in；It is described
Second acquisition module obtains the second laser frame rejected after dynamic laser point.
Further, the rejecting module includes
First coordinate unit, second laser frame in laser spots are obtained in local probability grid according to the pose at current time
The position coordinates of figure；
Second coordinate unit, determines second laser frame in laser spots in local probability grating map according to the position coordinates
On grid coordinate；
Judging unit, the probable value of grid judges whether the laser spots are dynamic laser point according to where grid coordinate；
Culling unit, it is that dynamic laser point is then picked that the judged result of judging unit, which rejects dynamic laser point, i.e. judged result,
Remove.
Further, described device also includes map rejuvenation module, for being updated according to optimal pose and second laser frame
Partial points cloud map and/or renewal local probability grating map.
Further, in addition to map reset module, for according to the displacement of sports equipment reset local probability grid
Lattice map.
Further, first acquisition module specifically for obtain current time more excellent pose, including
First acquisition unit, obtains the initial relative movement amount at the moment of sports equipment first 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, according to initial relative movement amount, and first laser frame and second laser frame, obtains interframe phase
To amount of exercise；
Second computing unit, calculates according to the pose of the first moment sports equipment and interframe amount of relative motion and obtains current
The more excellent pose at moment；
The present invention also provides another mileage counter device, including processor and the instruction executable for storing processor
Memory；
The processor is configured as：
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：
The odometer method that the present invention is provided, obtains initial relative movement amount and pose by code-disc revolution, passes through iteration
Nearest neighbor algorithm etc. carries out laser frame matching, and accurate amount of relative motion and pose are obtained with this.The position obtained with previous step
Appearance is initial value, and laser frame is matched with partial points cloud map, obtains more accurately pose and amount of exercise, and externally export.
This method can effectively suppress the accumulated error of odometer, to well adapting to property of dynamic environment without priori global map；Greatly
Scope closed loop (200 meters) error is within 1 meter.
The part that technical scheme substantially contributes to prior art in other words can be with software product
Form is embodied, and the computer software product can be stored in storage medium, such as ROM/RAM, magnetic disc, CD, including
Some instructions are to cause a computer equipment (can be personal computer, server, or network equipment etc.).
The mileage counter device that the present invention is provided, solve that existing existing odometer is inaccurate and cumulative errors not
Foot, available in numerous general or special purpose computing system environments or configuration.For example：Personal computer, server computer, hand
It is holding equipment or portable set, laptop device, multicomputer system, the system based on microprocessor, set top box, programmable
Consumer-elcetronics devices, network PC, minicom, mainframe computer include the Distributed Calculation of any of the above system or equipment
Environment etc..
Brief description of the drawings
Fig. 1 is the schematic flow sheet of odometer embodiment of the method 1 of the present invention；
Fig. 2 is the schematic flow sheet that odometer embodiment of the method 1 of the present invention obtains more excellent current pose；
Fig. 3 is the schematic flow sheet 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.
Embodiment
With reference to accompanying drawing and embodiment, the present invention is described further, it is necessary to explanation is, do not collided
On the premise of, new embodiment can be formed between various embodiments described below or between each technical characteristic in any combination：
Embodiment one
Fig. 1 is the schematic flow sheet for the odometer method that the present embodiment is provided.
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
Smart home to move and position indoors etc..Arithmetic facility is used to calculate various data, and data source is motion
Equipment, Laser Scanning Equipment, physics mileage equipment etc..Physics mileage equipment is used to detect that the exercise data for obtaining sports equipment comes
The sports equipment pose amount of changing over time, including position knots modification, angulation change amount etc. are estimated, sports equipment is generally positioned at
On, such as code-disc, other photoelectric encoders, or other can measure a period of time in sports equipment displacement detection
Element.
Certain zone of action typically is set with to sports equipment such as mobile robot.According to different sports equipments can
To set up different motion models, motion model, the motion model of Omni-mobile of differential driving common are.Specific real
Applying has two adjacent moment, i.e. previous moment and current time in example, typically 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 obtained according to the pose calculating at two momentt, Δ yt, Δ θt).For example for two-wheel
The motion model of difference, the motor of left and right two-wheeled is mounted on code-disc as physics mileage equipment, can pass through equation below
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 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, Δ d4The amount of exercise on four driving wheels respectively obtained by code-disc, r is wheel
Hub radius, rrFor roller radius, wheel base is 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 is obtained
Each laser frame data are presented by prior art in the form of laser point cloud.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 using Laser Scanning Equipment scanning local environment, is stored in arithmetic facility.It is local
Probabilistic Cell map, then be that partial points cloud map is divided into a series of grids, set up local probability grating map coordinate system, wherein
Each grid gives a probable value, represents the probability that the grid is occupied.Point cloud map coordinates system, grating map coordinate system with
Global coordinate system can be changed mutually, to determine pose of the sports equipment in map coordinates system.
Point in the setting up of coordinate system, 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, comprises the following steps：
Step S110：Obtain pose, the second laser frame and partial points cloud at current time at sports equipment current time
Map.
The current time is t, and the pose at the current time can be obtained by physics mileage equipment or prior art
Obtain the initial pose at current time
Simultaneously in current time t, obtained second laser frame is scanned by Laser Scanning Equipment, Laser Scanning Equipment will be obtained
The laser frame data obtained are sent to arithmetic facility, can also can specifically be set by wired or wireless mode by master control
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, arithmetic facility using the pose at current time be first
Value, carries out point cloud matching to second laser frame and partial points cloud map and obtains optimal pose, i.e., obtained most according to below equation
Optimization solution is optimal poseThe formula is：
Wherein：The curved surface constituted for partial points cloud map；piFor the laser spots of second laser frame in；SymbolGeneration
Table roto-translatory is converted；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, its final value is optimal pose
The second laser frame 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 by arithmetic facility by above-mentioned technical proposal calculate obtain it is current when
The pose at quarter, the pose obtained by prior art or physics mileage equipment more direct than step S110 is more accurately.
In the present embodiment, as a further improvement, the pose at the current time can also be by following technical side
The more excellent pose that case is obtainedFor replacing that by physics mileage equipment or prior art the initial bit at current time can be obtained
AppearanceFig. 2 is the schematic flow sheet that odometer embodiment of the method 1 of the present invention obtains more excellent current pose；Specifically, described more excellent
The acquisition of pose comprises the following steps：
Step S111：Obtain the initial relative movement amount at the moment of sports equipment first to current time.
First moment be current time before sometime point, generally current time previous moment t-1.Institute
Stating initial relative movement amount is used to describe the pose P residing for sports equipment from the first moment t-1t-1=(xt-1, yt-1, θt-1) mobile
To the location variation and angle variable quantity of the change of the pose residing for current time t, including sports equipment, useRepresent.
Wherein, the pose P residing for the first moment t-1t-1=(xt-1, yt-1, θt-1) known, it is preferable that first moment
Pose P residing for t-1t-1=(xt-1, yt-1, θt-1) it is the optimal pose that last odometer method is obtained.In a specific implementation
In, can be according to the pose P residing for the first moment t-1t-1=(xt-1, yt-1, θt-1) with current time t residing for initial poseCalculating is converted into initial relative movement amountComputational methods are existing skill
Art, and already described above.
The data obtained by physics mileage equipment or other prior arts are sent to arithmetic facility, pass through wired or nothing
The mode of line, can also be transmitted to arithmetic facility by total control equipment and be calculated.
Step S112：Obtain the first laser frame at the first moment and the second laser frame at current time.
In the first moment t-1, the laser frame that Laser Scanning Equipment scanning is obtained is first laser frame, in current time t, is swashed
The laser frame that optical scanning device scanning is obtained is second laser frame.
Step S113：According to initial relative movement amount, and first laser frame and second laser frame, the relative fortune of interframe is obtained
Momentum.
Specifically, the initial relative movement amount obtained using in step S111 is 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 below equation is interframe movement amount,
The formula is：
Wherein：The curved surface constituted for first laser frame；piFor the laser spots of second laser frame；Q is fortune in calculating process
The amount of relative motion of dynamic equipment, using amount of relative motion as initial value, its final value is interframe amount of relative motion；SymbolGeneration
Table roto-translatory is converted；SymbolRepresent the curved surface that point is constituted to first laser frameEuclid projection.
The frame matching of the first laser frame and second laser frame can be achieved by the prior art, for example iteration neighbour
Algorithm, violence matching etc..
The interframe amount of relative motion be calculated by arithmetic facility on the basis of initial relative movement amount obtain from the
Pose residing for one moment t-1 is moved to the variable quantity of the pose residing for current time t, first relative to what is obtained in step S111
Beginning amount of relative motion is more accurate.
Step S114：Calculated according to the pose of the first moment sports equipment and interframe amount of relative motion and obtain 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
Pose relative to the current time directly obtained by physics mileage equipment or other prior arts is more accurate.
Further, present invention additionally comprises 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 poseThe optimal amount of relative motion of acquisition can be calculated
, can be according to the first moment t-1 pose P in step S111 in the present inventiont-1=(xt-1, yt-1, θt-1) and it is current
Moment t poseCalculating 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 current time t initial poseIt can be achieved in step S114
The calculating of more excellent pose and interframe amount of relative motion is converted；Use optimal poseInstead of current time t poseStep can be achieved
Optimal pose is calculated with optimal amount of relative motion in rapid S131 converts.Meanwhile, can be with above-mentioned formula according to fortune by the present invention
The pose and optimal pose at the dynamic moment of equipment first, which are calculated, obtains optimal amount of relative motion.
The odometer method that the present invention is provided, obtains initial relative movement amount and pose by code-disc revolution, passes through iteration
Nearest neighbor algorithm etc. carries out laser frame matching, and accurate amount of relative motion and pose are obtained with this.The position obtained with previous step
Appearance is initial value, and laser frame is matched with partial points cloud map, obtains more accurately pose and amount of exercise, and externally export.
This method can effectively suppress the accumulated error of odometer, to well adapting to property of dynamic environment without priori global map；Greatly
Scope closed loop (200 meters) error is within 1 meter.
Embodiment two
Fig. 3 is the schematic flow sheet of odometer embodiment of the method 2 of the present invention.
The odometer method of the present embodiment includes
Step S210：Obtain second after 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, the probable value of Probabilistic Cell map is regard as the foundation for deleting dynamic point.By
Dual-laser frame is mapped to local probability grating map, if the probable value of grid where laser spots is less than the minimum probability of static grid
Value is then dynamic laser point for the laser spots in dynamic grid, the i.e. grid, specifically, is comprised the following steps：
Step S211：Second laser frame in laser spots are obtained in local probability grating map according to the pose at current time
Position coordinates；
Step S212：Determine second laser frame in laser spots on local probability grating map according to the position coordinates
Grid coordinate；
Step S213：The probable value of grid determines whether the laser spots are dynamic laser point according to where grid coordinate, such as
Fruit is then to reject dynamic laser point.
Principle is as follows：
Second laser frame in laser spots are laser spots i, and becoming second laser frame of changing commanders by roto-translatory is mapped to part generally
Rate grating map obtains laser spots i position coordinates Li, i.e.,
Grid where the origin O of local probability grating map, grid size reso, laser spots i grid coordinate (m,
N), then
Then as 0≤Bel (m, n)≤BelthresWhen, judge that laser spots i is dynamic laser point in the grid.Wherein, Bel (m,
N) it is the probable 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 is repeated no more.
Step S220：According to the pose at current time, and reject second laser frame and partial points after dynamic laser point
Cloud map, obtains the optimal pose at current time.
Specifically, using the pose at current time as initial value, to rejecting second laser frame and partial points cloud after dynamic point
Figure carries out point cloud matching and obtains optimal pose, i.e., the optimum solution obtained according to below equation is optimal poseThe public affairs
Formula is：
Wherein：The curved surface constituted for partial points cloud map；To reject the second laser frame in after dynamic laser point
Laser spots；P is the pose in calculating process, and using the pose at current time as initial value, its final value is optimal pose；Symbol
NumberRepresent roto-translatory conversion；SymbolRepresent the curved surface that point is constituted to partial points cloud mapEurope
It must be projected in several.
The in odometer method described in the present embodiment step S210 and step S220 unlike embodiment one
Dual-laser frame to reject the laser frame after dynamic laser point, step S210, step S220 and the current time in step S211
Pose can also use the more excellent pose at the current time of the method acquisition by step S111- steps S114.
In another embodiment, odometer method of the invention also includes step S240：Obtained most according to step S220
Excellent pose updates map with second laser frame, including updates 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 spots of second laser frame in are mapped in global coordinate system and obtain laser spots in world coordinates
The position of system.Wherein, laser spots i can be the laser spots for rejecting the second laser frame in after dynamic laser point, and laser spots i is complete
The position of office's coordinate systemBecome second laser frame of changing commanders by roto-translatory to be mapped in global coordinate system, i.e.,
Step S242：The position is inserted into partial points cloud map, the renewal of partial points cloud map is completed；This step can
To be achieved by the prior art, repeat no more here；
Step S243：In ray tracing mode, local probability grating map is updated；Including updating the grid near location point
Probable value and Laser Scanning Equipment and location point line on probable value.Specifically, typically with variance less Gauss point
Cloth, updates location pointThe probable value of neighbouring grid；By Laser Scanning Equipment and location pointGrid probability on line is put
Zero, zero represents that the probability that the grid is occupied is 0.
In another embodiment, the continuous accumulated distance of odometer, is used as another improvement of the present invention, in addition to step
S250 resets Probabilistic Cell map according to the displacement of sports equipment.Specifically, including sub-step：
Step S251：The displacement of sports equipment is calculated, and judges whether to reach setpoint distance；If reaching setpoint distance
Then carry out step S252, step S253, step S254 and carry out replacement map, the replacement grating map includes resetting grating map
Probable value, reset map origin, update local probability grating map.The displacement of the sports equipment passes through the present invention's
The odometer that odometer method is realized is obtained.
Step S252：All grid probable values are set to the probable value for representing unknown, general -1 represents 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 ray tracing mode with current time nearest N frame laser frames.
Update method is consistent with step S243, and the position of more accurately sports equipment can be obtained 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 acquisition module, obtains the pose at sports equipment current time；
Second acquisition module, obtains the second laser frame at sports equipment current time；
3rd acquisition module, obtains partial points cloud map；
First computing module, according to the pose at current time, and second laser frame and partial points cloud map, calculating is obtained
The optimal pose at current time.
In the present embodiment, as a further improvement, first acquisition module specifically for obtain current time compared with
Excellent pose pose, specifically, first acquisition module include
First acquisition unit, obtains the initial relative movement amount at the moment of sports equipment first 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, according to initial relative movement amount, and first laser frame and second laser frame, obtains interframe phase
To amount of exercise.
Second computing unit, calculates according to the pose of the first moment sports equipment and interframe amount of relative motion and obtains current
The more excellent pose at moment.
Further, mileage counter device of the invention also includes rejecting module, the dynamic for rejecting second laser frame in
Laser spots, second acquisition module obtains the second laser frame rejected after dynamic laser point.Specifically, the rejecting module bag
Include：
First coordinate unit, second laser frame in laser spots are obtained in local probability grid according to the pose at current time
The position coordinates of figure；
Second coordinate unit, determines second laser frame in laser spots in local probability grating map according to the position coordinates
On grid coordinate；
Judging unit, the probable value of grid judges whether the laser spots are dynamic laser point according to where grid coordinate,
Culling unit, it is that dynamic laser point is then picked that the judged result of judging unit, which rejects dynamic laser point, i.e. judged result,
Remove.
Further, mileage counter device of the invention also 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 includes：
Map unit, the laser spots of second laser frame in are mapped in global coordinate system, are obtained laser spots and are sat in the overall situation
Mark the position of system.Converted by roto-translatory, i.e., will by formulaThe laser of second laser frame in
Point is mapped in global coordinate system.
First updating block, is inserted into partial points cloud map by position, the renewal of partial points cloud map is completed with this.
Second updating block, in ray tracing mode, updates local probability grating map.Specifically, typically with variance compared with
Small Gaussian Profile, updates location pointThe probable value of neighbouring grid；By Laser Scanning Equipment and location pointGrid on line
Lattice probability zero setting, zero represents the probable value that the grid point is not occupied.
Further, mileage counter device of the invention also includes map replacement module, for the movement according to sports equipment
Distance resets local probability grating map.The map, which resets module, to be included
Metrics calculation unit, calculates the displacement of sports equipment；The computational methods are prior art, or pass through this
The odometer of method is calculated according to optimal pose and obtained.
Second judging unit, judges whether the displacement reaches setpoint distance；
If reaching setpoint distance, the map resets module and passes through probable value reset cell, origin reset cell, map weight
Put unit and reset local probability grating map.I.e. probable value reset cell, puts all grid probable values according to judged result
To represent unknown probable value；Origin reset cell, is ground according to judged result by position reset of the optimal pose in map
Primitive point；Map reset cell, according to judged result, in ray tracing mode, using current time nearest N frame laser frame more
New local probability grating map.
The method in device and previous embodiment in the present embodiment be based on two aspects under same inventive concept,
Above method implementation process is described in detail, thus those skilled in the art can according to it is described above clearly
The structure and implementation process of the system in this implementation are solved, it is succinct for specification, just repeat no more herein.
For convenience of description, it is divided into various modules during description apparatus above with function to describe respectively.Certainly, this is being implemented
The function of each module can be realized in same or multiple softwares and/or hardware during invention.
As seen through the above description of the embodiments, those skilled in the art can be understood that the present invention can
Realized by the mode of software plus required general hardware platform.Understood based on such, technical scheme essence
On the part that is contributed in other words to prior art can be embodied in the form of software product, the computer software product
It can be stored in storage medium, such as ROM/RAM, magnetic disc, CD, including some instructions are to cause a computer equipment
(can be personal computer, server, or network equipment etc.) performs some of each of the invention embodiment or embodiment
Method described in part.
The device embodiment of description is only schematical, wherein the module illustrated as separating component or unit can
To be or may not be physically separate, the part illustrated as module or unit can be or may not be thing
Module is managed, can both be located at a place, or can also be distributed on multiple mixed-media network modules mixed-medias.It can select according to the actual needs
Some or all of unit therein is selected to realize the purpose of this embodiment scheme.Those of ordinary skill in the art are not paying wound
In the case that the property made is worked, you can to understand and implement.
The present invention can be used in numerous general or special purpose computing system environments or configuration.For example：Personal computer, service
Device computer, handheld device or portable set, laptop device, multicomputer system, the system based on microprocessor, machine top
Box, programmable consumer-elcetronics devices, network PC, minicom, mainframe computer including any of the above system or equipment
DCE etc., such as example IV.
Example IV
Mileage counter device as depicted, including：Processor and the storage for storing the executable instruction of processor
Device；
The processor is configured as：
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, technical scheme that can be as described above and design, make other various corresponding
Change and deformation, and all these change and deformation should all belong to the claims in the present invention protection domain it
It is interior.
Claims (17)
1. a kind of odometer method, it is characterised 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 the pose at current time, and second laser frame and partial points cloud map, the optimal position for obtaining current time is calculated
Appearance.
2. odometer method according to claim 1, it is characterised in that the pose using sports equipment current time is first
Value, carries out point cloud matching to second laser frame and partial points cloud map and obtains optimal pose.
3. odometer method according to claim 2, it is characterised in that the optimal pose is following public especially by solving
The optimum solution of formula is obtained, and the formula is：
Wherein：The curved surface constituted for partial points cloud map；piFor the laser spots of second laser frame in；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.
4. odometer method according to claim 1, it is characterised in that the second laser frame is rejecting dynamic laser point
Second laser frame afterwards.
5. odometer method according to claim 4, it is characterised in that reject dynamic laser point in second laser frame
Step includes：
Position coordinates of the second laser frame in laser spots in local probability grating map is obtained according to the pose at current time；
Grid coordinate of the second laser frame in laser spots on local probability grating map is determined according to the position coordinates；
The probable value of grid determines whether the laser spots are dynamic laser point according to where grid coordinate, if it is rejects dynamic
Laser spots.
6. odometer method according to claim 1, it is characterised in that the pose at the current time is more excellent pose,
Specifically, the acquisition of the more excellent pose includes step：
Obtain the initial relative movement amount at the moment of sports equipment first 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；
7. odometer method according to claim 6, it is characterised in that using amount of relative motion as initial value, to first laser
Frame carries out frame matching with second laser frame and obtains interframe movement amount.
8. odometer method according to claim 7, it is characterised in that the interframe movement amount is following especially by solving
The optimum solution of formula is obtained, and the formula is：
Wherein：The curved surface constituted for first laser frame；piFor the laser spots of second laser frame；Q sets to be moved in calculating process
Standby amount of relative motion, using amount of relative motion as initial value；SymbolRepresent roto-translatory conversion；SymbolRepresent
The curved surface that point is constituted to first laser frameEuclid projection.
9. the odometer method according to claim any one of 1-8, it is characterised 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.
10. the odometer method according to claim any one of 1-8, it is characterised in that when the displacement of amount of relative motion reaches
During to given threshold, grating map is reset, the replacement grating map includes resetting the probable value of grating map, resets ground primitive
Point, renewal local probability grating map.
11. a kind of mileage counter device, it is characterised in that including
First acquisition module, obtains the pose at sports equipment current time；
Second acquisition module, obtains the second laser frame at sports equipment current time；
3rd acquisition module, obtains partial points cloud map；
First computing module, according to the pose at current time, and second laser frame and partial points cloud map, calculating obtains current
The optimal pose at moment.
12. mileage counter device according to claim 11, it is characterised in that described device also includes rejecting module, is used for
Reject the dynamic laser point of second laser frame in；Second acquisition module obtains the second laser rejected after dynamic laser point
Frame.
13. mileage counter device according to claim 12, it is characterised in that the rejecting module includes
First coordinate unit, second laser frame in laser spots are obtained in local probability grating map according to the pose at current time
Position coordinates；
Second coordinate unit, determines second laser frame in laser spots on local probability grating map according to the position coordinates
Grid coordinate；
Judging unit, the probable value of grid judges whether the laser spots are dynamic laser point according to where grid coordinate；
Culling unit, it is that dynamic laser point is then rejected that the judged result of judging unit, which rejects dynamic laser point, i.e. judged result,.
14. the mileage counter device according to claim any one of 11-13, it is characterised in that described device also includes map
Update module, for updating partial points cloud map according to optimal pose and second laser frame and/or with updating local probability grid
Figure.
15. the mileage counter device according to claim any one of 11-13, it is characterised in that also reset module including map,
For resetting local probability grating map according to the displacement of sports equipment.
16. the mileage counter device according to claim any one of 11-13, it is characterised in that the first acquisition module tool
Body is used for the more excellent pose for obtaining current time, including
First acquisition unit, obtains the initial relative movement amount at the moment of sports equipment first 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, according to initial relative movement amount, and first laser frame and second laser frame, obtains the relative fortune of interframe
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.
17. a kind of mileage counter device, it is characterised in that described mileage counter device includes：Processor and for storage processing
The memory of the executable instruction of device；
The processor is configured as：
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 true CN107144292A (en) | 2017-09-08 |
| CN107144292B 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) |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108253958A (en) * | 2018-01-18 | 2018-07-06 | 亿嘉和科技股份有限公司 | A kind of robot real-time location method under sparse environment |
| CN108844553A (en) * | 2018-06-27 | 2018-11-20 | 广州视源电子科技股份有限公司 | Method and device for correcting mileage in robot moving process and robot |
| CN109631919A (en) * | 2018-12-28 | 2019-04-16 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of hybrid navigation map constructing method for merging reflector and occupying grid |
| CN110824496A (en) * | 2019-09-18 | 2020-02-21 | 北京迈格威科技有限公司 | Motion estimation method, motion estimation device, computer equipment and storage medium |
| CN110887493A (en) * | 2019-11-29 | 2020-03-17 | 上海有个机器人有限公司 | Trajectory estimation method, medium, terminal and device based on local map matching |
| CN110969649A (en) * | 2019-11-29 | 2020-04-07 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device of laser point cloud and map |
| CN111609853A (en) * | 2019-02-25 | 2020-09-01 | 北京奇虎科技有限公司 | Three-dimensional map construction method, sweeping robot and electronic equipment |
| CN111637877A (en) * | 2020-05-29 | 2020-09-08 | 拉扎斯网络科技（上海）有限公司 | Robot positioning method and device, electronic equipment and nonvolatile storage medium |
| CN111696159A (en) * | 2020-06-15 | 2020-09-22 | 湖北亿咖通科技有限公司 | Feature storage method of laser odometer, electronic device and storage medium |
| CN111735451A (en) * | 2020-04-16 | 2020-10-02 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
| CN112113574A (en) * | 2020-03-02 | 2020-12-22 | 北京百度网讯科技有限公司 | Method, apparatus, computing device and computer-readable storage medium for positioning |
| CN112344940A (en) * | 2020-11-06 | 2021-02-09 | 杭州国辰机器人科技有限公司 | Positioning method and device integrating reflective columns and grid map |
| CN112700495A (en) * | 2020-11-25 | 2021-04-23 | 北京旷视机器人技术有限公司 | Pose determination method and device, robot, electronic device and storage medium |
| CN113012191A (en) * | 2021-03-11 | 2021-06-22 | 中国科学技术大学 | 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 |
| CN117170354A (en) * | 2022-10-25 | 2023-12-05 | 中科南京软件技术研究院 | 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 |
|---|
| 赵梓乔等: "基于三维激光扫描仪的室内移动设备定位与建图", 《计算机与数字工程》 * |
Cited By (26)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108253958A (en) * | 2018-01-18 | 2018-07-06 | 亿嘉和科技股份有限公司 | A kind of robot real-time location method under sparse environment |
| CN108844553A (en) * | 2018-06-27 | 2018-11-20 | 广州视源电子科技股份有限公司 | Method and device for correcting mileage in robot moving process and robot |
| CN108844553B (en) * | 2018-06-27 | 2021-06-15 | 广州视源电子科技股份有限公司 | Method and device for correcting mileage in robot moving process and robot |
| CN109631919A (en) * | 2018-12-28 | 2019-04-16 | 芜湖哈特机器人产业技术研究院有限公司 | A kind of hybrid navigation map constructing method for merging reflector and occupying grid |
| CN109631919B (en) * | 2018-12-28 | 2022-09-30 | 芜湖哈特机器人产业技术研究院有限公司 | Hybrid navigation map construction method integrating reflector and occupied grid |
| CN111609853A (en) * | 2019-02-25 | 2020-09-01 | 北京奇虎科技有限公司 | Three-dimensional map construction method, sweeping robot and electronic equipment |
| CN110824496A (en) * | 2019-09-18 | 2020-02-21 | 北京迈格威科技有限公司 | Motion estimation method, motion estimation device, computer equipment and storage medium |
| CN110969649A (en) * | 2019-11-29 | 2020-04-07 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device of laser point cloud and map |
| CN110969649B (en) * | 2019-11-29 | 2024-04-05 | 上海有个机器人有限公司 | Matching evaluation method, medium, terminal and device for laser point cloud and map |
| CN110887493A (en) * | 2019-11-29 | 2020-03-17 | 上海有个机器人有限公司 | Trajectory estimation method, medium, terminal and device based on local map matching |
| CN110887493B (en) * | 2019-11-29 | 2023-05-05 | 上海有个机器人有限公司 | Track calculation method, medium, terminal and device based on local map matching |
| CN112113574A (en) * | 2020-03-02 | 2020-12-22 | 北京百度网讯科技有限公司 | Method, apparatus, computing device and computer-readable storage medium for positioning |
| 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 |
| CN112113574B (en) * | 2020-03-02 | 2022-10-11 | 北京百度网讯科技有限公司 | Method, apparatus, computing device and computer-readable storage medium for positioning |
| CN111735451A (en) * | 2020-04-16 | 2020-10-02 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
| CN111735451B (en) * | 2020-04-16 | 2022-06-07 | 中国北方车辆研究所 | Point cloud matching high-precision positioning method based on multi-source prior information |
| CN111637877A (en) * | 2020-05-29 | 2020-09-08 | 拉扎斯网络科技（上海）有限公司 | Robot positioning method and device, electronic equipment and nonvolatile storage medium |
| CN111696159A (en) * | 2020-06-15 | 2020-09-22 | 湖北亿咖通科技有限公司 | Feature storage method of laser odometer, electronic device and storage medium |
| CN112344940A (en) * | 2020-11-06 | 2021-02-09 | 杭州国辰机器人科技有限公司 | Positioning method and device integrating reflective columns and grid map |
| CN112344940B (en) * | 2020-11-06 | 2022-05-17 | 杭州国辰机器人科技有限公司 | Positioning method and device integrating reflective columns and grid map |
| CN112700495A (en) * | 2020-11-25 | 2021-04-23 | 北京旷视机器人技术有限公司 | Pose determination method and device, robot, electronic device and storage medium |
| CN113012191B (en) * | 2021-03-11 | 2022-09-02 | 中国科学技术大学 | Laser mileage calculation method based on point cloud multi-view projection graph |
| CN113012191A (en) * | 2021-03-11 | 2021-06-22 | 中国科学技术大学 | 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 |
| CN117170354A (en) * | 2022-10-25 | 2023-12-05 | 中科南京软件技术研究院 | Wheel type robot positioning method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107144292B (en) | 2019-10-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107144292B (en) | A kind of the odometer method and mileage counter device of sports equipment | |
| Dai et al. | Fast frontier-based information-driven autonomous exploration with an mav | |
| CN105247431B (en) | Autonomous body | |
| US11086016B2 (en) | Method and apparatus for tracking obstacle | |
| US10823576B2 (en) | Systems and methods for robotic mapping | |
| Philipp et al. | Mapgenie: Grammar-enhanced indoor map construction from crowd-sourced data | |
| CN103925923B (en) | A kind of earth magnetism indoor locating system based on adaptive particle filter device algorithm | |
| CN106573372B (en) | Charger, the method, apparatus and system that charger is found based on map structuring | |
| Pan et al. | Voxfield: Non-projective signed distance fields for online planning and 3d reconstruction | |
| CN110501017A (en) | A kind of Mobile Robotics Navigation based on ORB_SLAM2 ground drawing generating method | |
| 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 | |
| Xiao et al. | Monocular vehicle self-localization method based on compact semantic map | |
| CN103389738A (en) | Method and device for table tennis robot to predict table tennis ball trajectory | |
| CN103412565A (en) | robot with global location rapid estimating capability and positioning method thereof | |
| Faigl et al. | A sensor placement algorithm for a mobile robot inspection planning | |
| CN105704652A (en) | Method for building and optimizing fingerprint database in WLAN/Bluetooth positioning processes | |
| CN110375712A (en) | Drift section extracting method, device, equipment and storage medium | |
| CN103901891A (en) | Dynamic particle tree SLAM algorithm based on hierarchical structure | |
| Liu et al. | Real-time 6d lidar slam in large scale natural terrains for ugv | |
| Safavi et al. | An opportunistic linear–convex algorithm for localization in mobile robot networks | |
| CN104992466A (en) | Instant route-finding method for three-dimensional scene | |
| Hu et al. | PALoc: Advancing SLAM Benchmarking With Prior-Assisted 6-DoF Trajectory Generation and Uncertainty Estimation | |
| CN116858269A (en) | Tobacco industry finished product warehouse flat warehouse inventory robot path optimization method based on laser SLAM | |
| CN105806331A (en) | Positioning method for indoor robot and indoor robot |
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. |