CN107144292A - The odometer method and mileage counter device of a kind of sports equipment - Google Patents

Abstract

The invention discloses an odometer method and an odometer device for sports equipment, which include obtaining the pose of the sports equipment at the current moment, a second laser frame at the current moment, and a local point cloud map; according to the pose at the current moment, and the second The laser frame and the local point cloud map are calculated to obtain the optimal pose at the current moment. The odometer method provided by the present invention obtains the initial relative motion and pose through the number of code wheel revolutions, and performs laser frame interpolation through iterative neighbor algorithms. Matching, in order to obtain more accurate relative motion and pose. The pose obtained in the previous step is the initial value, and the laser frame is matched with the local point cloud map to obtain a more accurate pose and motion, and output it externally. The odometer device provided by the invention solves the shortcomings of inaccuracy and cumulative error of existing odometers, and can be used in many general or special computing system environments or configurations.

Description

Odometer method and odometer device for sports equipment

technical field

The invention belongs to the technical field of sports equipment, in particular to an odometer method and an odometer device for the sports equipment.

Background technique

When a mobile robot moves in an indoor environment, the odometer information is the most basic information of the robot's motion state. A robot's odometry is a method that uses actuator movement data to estimate how much a robot's pose has changed over time. This pose change can be obtained by encoders and inertial navigation sensors. The traditional encoder estimating method only uses the accumulation of internal sensor information to obtain mileage information. For example, a robot is equipped with rotary encoders at its wheels or leg joints. When it moves forward for a period of time, it wants to know the approximate distance. Moving distance, with the help of a rotary encoder, the number of turns the wheel rotates can be measured, and if the circumference of the wheel is known, the distance the robot moves can be calculated.

But such odometers always suffer from accuracy problems. Since the error is caused by a mixture of many factors, and the accumulation of error over time leads to the odometer reading becoming less and less reliable as time goes by.

Contents of the invention

In order to solve the defect of low precision and high error of the odometer in the prior art, the present invention provides an odometer method and an odometer device for sports equipment.

The present invention is achieved through the following technical solutions: acquiring the pose of the motion equipment at the current moment, the second laser frame at the current moment, and the local point cloud map;

According to the pose at the current moment, as well as the second laser frame and the local point cloud map, the optimal pose at the current moment is calculated.

Further, with the current pose of the sports equipment as the initial value, point cloud matching is performed on the second laser frame and the local point cloud map to obtain the optimal pose, that is, the optimal pose is the optimal pose obtained according to the following formula Solution, the formula is:

in: is the surface formed by the local point cloud map; p _i is the laser point in the second laser frame; the symbol Represents a rotation-translation transformation; the symbol Surfaces representing points to local point cloud maps The Euclidean projection of ; P is the pose during the calculation process, with the pose at the current moment as the initial value.

Further, the second laser frame is the second laser frame after excluding dynamic laser points.

Further, the step of eliminating the dynamic laser points in the second laser frame includes: obtaining the position coordinates of the laser points in the second laser frame in the local probability grid map according to the pose at the current moment; determining the second laser point according to the position coordinates The grid coordinates of the laser point in the frame on the local probability grid map; determine whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located, and if so, remove the dynamic laser point.

Further, the pose at the current moment is a better pose. Specifically, the acquisition of the better pose includes the steps of: acquiring the initial relative motion of the sports equipment from the first moment to the current moment; A laser frame and the second laser frame at the current moment; according to the initial relative motion amount, and the first laser frame and the second laser frame, the relative motion amount between frames is obtained; calculated according to the pose of the moving device at the first moment and the relative motion amount between frames The best pose at the current moment.

Further, with the relative motion amount as an initial value, inter-frame matching is performed on the first laser frame and the second laser frame to obtain an inter-frame motion amount.

Further, the inter-frame motion amount is specifically obtained by solving the optimal solution of the following formula, which is:

in: is the curved surface formed by the first laser frame; p _i is the laser point of the second laser frame; q is the relative motion of the moving device during the calculation process, with the relative motion as the initial value; the symbol Represents a rotation-translation transformation; the symbol represents the surface formed from the point to the first laser frame Euclidean projection.

Further, updating the map according to the optimal pose and the second laser frame includes updating the local point cloud map and/or updating the local probability grid map.

Further, when the displacement of the relative movement amount reaches a set threshold, the grid map is reset, and the grid map reset includes resetting the probability value of the grid map, resetting the origin of the map, and updating the local probability grid map.

The present invention also provides an odometer device, the technical solution of which is that the odometer device includes

The first acquisition module acquires the pose of the sports equipment at the current moment;

The second acquisition module acquires the second laser frame at the current moment of the sports equipment;

The third acquisition module acquires the local point cloud map;

The first calculation module calculates the optimal pose at the current moment according to the pose at the current moment, as well as the second laser frame and the local point cloud map;

Further, the device further includes a removing module, configured to remove dynamic laser points in the second laser frame; the second acquisition module obtains the second laser frame after removing the dynamic laser points.

Further, the elimination module includes

The first coordinate unit obtains the position coordinates of the laser point in the local probability grid map in the second laser frame according to the pose at the current moment;

The second coordinate unit determines the grid coordinates of the laser point in the second laser frame on the local probability grid map according to the position coordinates;

The judging unit judges whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located;

The rejecting unit, the judgment result of the judging unit rejects the dynamic laser spot, that is, the judgment result is a dynamic laser spot, and then the dynamic laser spot is rejected.

Further, the device further includes a map update module, configured to update the local point cloud map and/or update the local probability grid map according to the optimal pose and the second laser frame.

Further, a map reset module is also included, which is used to reset the local probability grid map according to the moving distance of the sports equipment.

Further, the first acquisition module is specifically used to acquire a better pose at the current moment, including

The first acquisition unit acquires the initial relative movement amount of the sports equipment from the first moment to the current moment;

The second acquiring unit is configured to acquire the first laser frame at the first moment and the second laser frame at the current moment;

The first calculation unit obtains the relative motion between frames according to the initial relative motion, and the first laser frame and the second laser frame;

The second calculation unit calculates and obtains the optimal pose at the current moment according to the pose of the moving device at the first moment and the relative movement amount between frames;

The present invention also provides another odometer device, which includes a processor and a memory for storing instructions executable by the processor;

The processor is configured to:

Obtain the pose of the motion equipment at the current moment, the second laser frame at the current moment, and the local point cloud map,

According to the pose at the current moment, as well as the second laser frame and the local point cloud map, the optimal pose at the current moment is obtained. The beneficial effects of the present invention are as follows:

The odometer method provided by the present invention obtains the initial relative movement amount and pose through the rotation number of the code wheel, and performs laser frame-to-frame matching through an iterative neighbor algorithm to obtain more accurate relative movement amount and pose. The pose obtained in the previous step is the initial value, and the laser frame is matched with the local point cloud map to obtain a more accurate pose and motion, and output it externally. This method does not require a priori global map, can effectively suppress the cumulative error of the odometer, and has good adaptability to dynamic environments; the error of a large-scale closed loop (200 meters) is within 1 meter.

The essence of the technical solution of the present invention or the part that contributes to the prior art can be embodied in the form of software products, and the computer software products can be stored in storage media, such as ROM/RAM, magnetic disks, optical disks, etc., including Several instructions are used to make a computer device (it may be a personal computer, a server, or a network device, etc.).

The odometer device provided by the invention solves the inaccuracy and accumulated errors of existing odometers, and can be used in many general or special computing system environments or configurations. Examples: personal computers, server computers, handheld or portable devices, tablet-type devices, multiprocessor systems, microprocessor-based systems, set-top boxes, programmable consumer electronics, network PCs, minicomputers, mainframe computers, including the above A distributed computing environment for any system or device, and more.

Description of drawings

Fig. 1 is the schematic flow sheet of embodiment 1 of odometer method of the present invention;

Fig. 2 is a schematic flow diagram of obtaining a better current pose in Embodiment 1 of the odometer method of the present invention;

Fig. 3 is a schematic flow chart of embodiment 2 of the odometer method of the present invention;

Fig. 4 is a schematic diagram of the module structure of the odometer device of the present invention.

detailed description

The present invention will be further described in conjunction with the accompanying drawings and specific implementation methods. It should be noted that, on the premise of not conflicting, the various embodiments described below or the technical features can be combined arbitrarily to form new embodiments:

Embodiment one

Fig. 1 is a schematic flowchart of the odometer method provided in this embodiment.

The devices in the application scene of this method include sports equipment, computing equipment, and laser scanning equipment and physical mileage equipment set on the sports equipment. Sports equipment can be mobile robots, sweeping robots, robotic toys, or smart homes that need to be moved and positioned indoors. Computing equipment is used to calculate various data, and the data sources are sports equipment, laser scanning equipment, physical mileage equipment, etc. The physical mileage device is used to detect and obtain the motion data of the sports equipment to estimate the change in the pose of the sports equipment over time, including the position change, angle change, etc., and is generally set on the sports equipment, such as a code wheel, other photoelectric encoders, or Other sensing elements that measure the displacement of moving equipment over a period of time.

Generally, a certain activity area is set for sports equipment such as a mobile robot. Different motion models can be established according to different motion equipment, common ones are differential drive motion model and omnidirectional motion motion model. In a specific embodiment, there are two adjacent moments, that is, the previous moment and the current moment. Generally, according to different motion models and the known pose (x _t-1 , y _t-1 , θ _t-1 ), the pose (x _t , y _t , θ _t ) of the motion device at the current moment can be obtained, and the relative motion amount (Δx _t , Δy _t , Δθ _t ) can be calculated according to the pose at two moments . For example, for the motion model of two-wheel differential, the drive motors of the left and right wheels are equipped with code discs as physical mileage devices, and the pose (x _t , y _t , θ _t ) at the current moment can be obtained by the following formula

ΔD _t = (Δd _L +Δd _R )/2

Δθ _t = (Δd _L -Δd _R )/ω

θ _t = θ _t-1 + Δθ _t

Among them, the distance between the left and right wheels is ω, ΔD _t is the distance change of the sports equipment calculated according to the information of the code disc, Δθ _t is the angle change of the sports equipment, Δd _L and Δd _R are respectively passed The amount of movement of the left and right wheels obtained by the left and right code discs;

Another example is that the driving motors of the four driving wheels of the omnidirectional moving motion model are all equipped with code discs, and the pose (x _t , y _t , θ _t ) at the current moment can be obtained by the following formula

x _t = x _t - ₁ +Δx

y _t =y _t-1 +Δy

θ _t = θ _t-1 + Δθ

Among them, Δd ₁ , Δd ₂ , Δd ₃ , and Δd ₄ are the motions on the four driving wheels obtained by the code disc, r is the hub radius, r _r is the roller radius, the front and rear wheelbase is 2l _x , and the left and right wheelbase is 2l _y ;

The scanning direction of the laser scanning device is in front of the moving direction of the moving device, and each laser frame data scanned by the laser scanning device is presented in the form of a laser point cloud through the existing technology. Set up a global coordinate system, the pose of the motion equipment, the position of the laser scanning device, and the obtained laser frame data can be unified into the global coordinate system.

The local point cloud map is formed by scanning the local environment with a laser scanning device and stored in the computing device. The local probability grid map is to divide the local point cloud map into a series of grids, and establish a local probability grid map coordinate system, in which each grid is given a possible value, indicating the probability that the grid is occupied. The point cloud map coordinate system, the grid map coordinate system and the global coordinate system can be converted to each other to determine the pose of the mobile device in the map coordinate system.

The establishment of the coordinate system, the coordinate transformation and the mapping of the points in the coordinate system to other coordinate systems can all be realized by existing technologies.

The odometer method as shown in Figure 1 comprises the following steps:

Step S110: Obtain the pose of the motion device at the current moment, the second laser frame at the current moment, and the local point cloud map.

The current moment is t, and the pose at the current moment is the initial pose at the current moment that can be obtained through physical mileage equipment or existing technologies

At the same time, at the current time t, the laser scanning device scans the second laser frame data, and the laser scanning device sends the obtained laser frame data to the computing device. Specifically, it can be wired or wireless, or the master control device can send the laser frame data to the computing device. The frame data is forwarded to the computing device.

Step S120: Obtain the optimal pose at the current moment according to the pose at the current moment, as well as the second laser frame and the local point cloud map.

Specifically, the local point cloud map is stored in the computing device, and the computing device uses the pose at the current moment as an initial value to perform point cloud matching on the second laser frame and the local point cloud map to obtain the optimal pose, that is, according to the following The optimal solution obtained by the formula is the optimal pose The formula is:

in: is the surface formed by the local point cloud map; p _i is the laser point in the second laser frame; the symbol Represents a rotation-translation transformation; the symbol Surfaces representing points to local point cloud maps The Euclidean projection of ; P is the pose during the calculation process, with the pose at the current moment as the initial value, and its final value is the optimal pose

The point cloud matching of the second laser frame and the local point cloud map can be realized by existing technologies, such as iterative nearest neighbor algorithm, brute force matching, and the like. The optimal pose is the pose at the current moment calculated and obtained by the computing device through the above technical solution, which is more accurate than the pose directly obtained by the prior art or the physical mileage device in step S110.

In this embodiment, as a further improvement, the pose at the current moment may also be a better pose obtained through the following technical solutions It is used to replace the initial pose obtained at the current moment through physical mileage equipment or existing technologies Fig. 2 is a schematic flow chart of obtaining a better current pose in Embodiment 1 of the odometer method of the present invention; specifically, the acquisition of the better pose includes the following steps:

Step S111: Obtain the initial relative exercise amount of the exercise device from the first moment to the current moment.

The first moment is a certain point in time before the current moment, generally a moment t-1 before the current moment. The initial relative movement amount is used to describe that the moving device moves from the pose P _t-1 =(x _t-1 , y _t-1 , θ _t-1 ) at the first moment t-1 to the current moment t The change of the pose, including the position change and angle change of the sports equipment, is used express.

Wherein, the pose P _{t-1 at the first moment t-1} = (x _t-1 , y _t-1 , θ _t-1 ) is known, preferably, the position at the first moment t-1 The pose P _t-1 = (x _t-1 , y _t-1 , θ _t-1 ) is the optimal pose obtained by the last odometry method. In a specific implementation, according to the pose P _t-1 =(x _t-1 , y _t-1 , θ _t-1 ) at the first moment t-1 and the initial pose at the current moment t The calculation is converted into the initial relative motion The calculation method is prior art and has been described above.

The data obtained through the physical mileage device or other existing technologies are sent to the computing device, and can also be forwarded to the computing device through the master control device for calculation through wired or wireless methods.

Step S112: Obtain the first laser frame at the first moment and the second laser frame at the current moment.

At the first time t−1, the laser frame scanned by the laser scanning device is the first laser frame, and at the current time t, the laser frame scanned by the laser scanning device is the second laser frame.

Step S113: According to the initial relative motion, and the first laser frame and the second laser frame, obtain the inter-frame relative motion.

Specifically, with the initial relative motion obtained in step S111 as the initial value, inter-frame matching is performed on the first laser frame and the second laser frame to obtain the inter-frame relative motion, that is, the optimal solution obtained according to the following formula is the inter-frame motion , the formula is:

in: is the curved surface formed by the first laser frame; p _i is the laser point of the second laser frame; q is the relative motion of the moving device during the calculation process, with the relative motion as the initial value, and the final value is the relative motion between frames; symbol Represents a rotation-translation transformation; the symbol represents the surface formed from the point to the first laser frame Euclidean projection.

The frame-to-frame matching between the first laser frame and the second laser frame can be realized by existing technologies, such as iterative nearest neighbor algorithm, brute force matching, and the like.

The inter-frame relative motion amount is the change amount calculated by the computing device on the basis of the initial relative motion amount from the pose at the first moment t-1 to the pose at the current moment t, relative to step S111 The initial relative motion obtained in is more accurate.

Step S114: Calculate and obtain a better pose at the current moment based on the pose of the moving device at the first moment and the relative motion between frames.

The pose of the moving device at the first moment is known, according to the pose of the moving device at the first moment t-1 P _t-1 = (x _t-1 , y _t-1 , θ _t-1 ), and the relative motion between frames Can calculate the better pose converted to the current moment The optimal pose Compared with the current pose obtained directly through physical mileage equipment or other existing technologies, it is more accurate.

Further, the present invention also includes step S131: calculating and converting into an optimal relative exercise amount according to the pose and the optimal pose of the sports equipment at the first moment.

The pose of the sports equipment at the first moment is known, according to the pose of the sports equipment at the first moment t-1 P _t-1 = (x _t-1 , y _t-1 , θ _t-1 ), and the optimal pose The optimal relative exercise can be calculated

In the present invention, in step S111, according to the pose P _t-1 = (x _t-1 , y _t-1 , θ _t-1 ) at the first moment t-1 and the pose at the current moment t The calculation of is transformed into the initial relative movement amount, and the calculation formula is as follows:

x _t ＝x _t-1 +cos(θ _t-1 )Δx _t -sin(θ _t-1 )Δy _t

y _t ＝y _t-1 +sin(θ _t-1 )Δx _t +cos(θ _t-1 )Δy _t

θ _t = θ _t-1 + Δθ _t

Among them, using the better pose at the current moment Replace the initial pose at the current moment t The calculation conversion of the optimal pose and the relative motion between frames can be realized in step S114; with the optimal pose Replace the pose at the current moment t The conversion between the optimal pose and the optimal relative motion in step S131 can be realized. At the same time, through the present invention, the above formula can also be used to calculate and obtain the optimal relative exercise amount according to the posture and the optimal posture of the sports equipment at the first moment.

The odometer method provided by the present invention obtains the initial relative movement amount and pose through the rotation number of the code wheel, and performs laser frame-to-frame matching through an iterative neighbor algorithm to obtain more accurate relative movement amount and pose. The pose obtained in the previous step is the initial value, and the laser frame is matched with the local point cloud map to obtain a more accurate pose and motion, and output it externally. This method does not require a priori global map, can effectively suppress the cumulative error of the odometer, and has good adaptability to dynamic environments; the error of a large-scale closed loop (200 meters) is within 1 meter.

Embodiment two

Fig. 3 is a schematic flowchart of Embodiment 2 of the odometer method of the present invention.

The odometer method of the present embodiment includes

Step S210: Obtain the current pose of the sports device, the current second laser frame after excluding laser dynamic points, and the local point cloud map.

A local probability grid map is introduced, and the probability value of the probability grid map is used as the basis for deleting dynamic points. Map the second laser frame to the local probability grid map. If the probability value of the grid where the laser point is located is less than the lowest probability value of the static grid, it is a dynamic grid, that is, the laser points in the grid are all dynamic laser points. Specifically, the following steps are included:

Step S211: Obtain the position coordinates of the laser point in the local probability grid map in the second laser frame according to the pose at the current moment;

Step S212: Determine the grid coordinates of the laser point in the second laser frame on the local probability grid map according to the position coordinates;

Step S213: Determine whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located, and if so, reject the dynamic laser point.

The principle is as follows:

The laser point in the second laser frame is laser point i, and the second laser frame is mapped to the local probability grid map by rotation-translation transformation to obtain the position coordinate L _i of laser point i, namely

The origin coordinate O of the local probability grid map, the grid size reso, and the grid coordinates (m, n) of the grid where the laser point i is located, then

Then when 0≤Bel(m,n)≤Bel _thres , it is determined that the laser point i in the grid is a dynamic laser point. Among them, Bel(m, n) is the probability value of the grid, Bel _thres is the lowest probability value of the static grid, Bel(m, n), the value of Bel _thres , and the elimination of dynamic laser points are all realized through the existing technology, The present invention will not be described in detail.

Step S220: Obtain the optimal pose at the current moment according to the pose at the current moment, the second laser frame and the local point cloud map after removing the dynamic laser points.

Specifically, with the current pose as the initial value, point cloud matching is performed on the second laser frame after excluding dynamic points and the local point cloud map to obtain the optimal pose, that is, the optimal solution obtained according to the following formula is the optimal Excellent pose The formula is:

in: A surface composed of a local point cloud map; is the laser point in the second laser frame after excluding the dynamic laser point; P is the pose during the calculation process, the pose at the current moment is taken as the initial value, and the final value is the optimal pose; the symbol Represents a rotation-translation transformation; the symbol Surfaces representing points to local point cloud maps Euclidean projection.

The difference between the odometer method described in this embodiment and the first embodiment is that the second laser frame in step S210 and step S220 is the laser frame after removing the dynamic laser point, and the current moment in step S210, step S220 and step S211 The pose of can also use the better pose at the current moment obtained through the method of step S111-step S114.

In another embodiment, the odometry method of the present invention further includes step S240: updating the map according to the optimal pose obtained in step S220 and the second laser frame, including updating the local probability grid map and updating the local point cloud map. The local probability grid map and local point cloud map obtained in this step are applied to the next implementation of the odometry method to obtain more accurate odometry data, which specifically includes the following sub-steps:

Step S241: Map the laser point in the second laser frame to the global coordinate system to obtain the position of the laser point in the global coordinate system. Among them, the laser point i can be the laser point in the second laser frame after excluding the dynamic laser point, and the position of the laser point i in the global coordinate system Map the second laser frame into the global coordinate system by rotation-translation transformation, namely

Step S242: Insert the position into the local point cloud map to complete the update of the local point cloud map; this step can be realized by existing technologies, and will not be repeated here;

Step S243: Updating the local probability grid map by means of ray tracing; including updating the probability value of the grid near the location point and the probability value on the line between the laser scanning device and the location point. Specifically, the location point is generally updated with a Gaussian distribution with a small variance Probability values for nearby rasters; laser scan device and location points The grid probability on the connection line is set to zero, and zero means that the probability of the grid being occupied is 0.

In another embodiment, the odometer continuously accumulates mileage. As another improvement of the present invention, step S250 is further included to reset the probability grid map according to the moving distance of the sports equipment. Specifically, including sub-steps:

Step S251: Calculate the moving distance of the sports equipment, and judge whether it has reached the set distance; if the set distance is reached, proceed to step S252, step S253, and step S254 to reset the map, and the reset grid map includes the reset grid The probability value of the map, reset the map origin, and update the local probability grid map. The moving distance of the sports equipment is obtained by the odometer realized by the odometer method of the present invention.

Step S252: set the probability values of all the grids to represent unknown probability values, generally -1 indicates that the probability value of the grid is unknown, that is, it is not known whether the grid is occupied;

Step S253: Determine the map origin according to the position of the optimal pose in the map; specifically, take the position of the optimal pose in the map or a position shifted by a certain distance as the map origin;

Step S254: Update the local probability grid map by ray tracing with the latest N frames of laser frames at the current moment. The update method is consistent with step S243, so that a more accurate pose of the sports equipment can be obtained in the next odometer method.

Embodiment Three

Fig. 4 is a schematic diagram of the module structure of the odometer device of the present invention.

An odometer device comprising

The first acquisition module acquires the pose of the sports equipment at the current moment;

The second acquisition module acquires the second laser frame at the current moment of the sports equipment;

The third acquisition module acquires the local point cloud map;

The first calculation module calculates the optimal pose at the current moment according to the pose at the current moment, as well as the second laser frame and the local point cloud map.

In this embodiment, as a further improvement, the first acquisition module is specifically configured to acquire a better pose at the current moment, specifically, the first acquisition module includes

The first acquisition unit acquires the initial relative exercise amount of the sports equipment from the first moment to the current moment.

The second acquiring unit acquires the first laser frame at the first moment and the second laser frame at the current moment.

The first calculation unit obtains the inter-frame relative motion according to the initial relative motion, and the first laser frame and the second laser frame.

The second calculation unit calculates and obtains the optimal pose at the current moment according to the pose of the moving device at the first moment and the relative motion between frames.

Further, the odometer device of the present invention further includes a removing module, configured to remove dynamic laser points in the second laser frame, and the second acquisition module obtains the second laser frame after removing the dynamic laser points. Specifically, the elimination module includes:

The first coordinate unit obtains the position coordinates of the laser point in the local probability grid map in the second laser frame according to the pose at the current moment;

The second coordinate unit determines the grid coordinates of the laser point in the second laser frame on the local probability grid map according to the position coordinates;

The judging unit judges whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located,

The rejecting unit, the judgment result of the judging unit rejects the dynamic laser spot, that is, the judgment result is a dynamic laser spot, and then the dynamic laser spot is rejected.

Further, the odometer device of the present invention also includes

It is used to update the local point cloud map and the local probability grid map according to the optimal pose and the second laser frame. Specifically, the map update module includes:

The mapping unit maps the laser point in the second laser frame to the global coordinate system to obtain the position of the laser point in the global coordinate system. Through the rotation-translation transformation, that is, the formula will The laser points within the second laser frame are mapped into the global coordinate system.

The first update unit inserts the position into the local point cloud map, so as to complete the update of the local point cloud map.

The second update unit updates the local probability grid map in a ray tracing manner. Specifically, the location point is generally updated with a Gaussian distribution with a small variance Probability values for nearby rasters; laser scan device and location points The grid probability on the connecting line is set to zero, and zero represents the probability value that the grid point is not occupied.

Further, the odometer device of the present invention further includes a map reset module, configured to reset the local probability grid map according to the moving distance of the sports equipment. The map reset module includes

The distance calculation unit is used to calculate the moving distance of the sports equipment; the calculation method is the prior art, or is obtained through the odometer calculation of the method according to the optimal pose.

a second judging unit, judging whether the moving distance reaches a set distance;

If the set distance is reached, the map reset module resets the local probability grid map through the probability value reset unit, the origin reset unit, and the map reset unit. That is, the probability value reset unit, which sets all the grid probability values as unknown probability values according to the judgment result; the origin reset unit, which resets the position of the optimal pose in the map to the map origin according to the judgment result; The reset unit updates the local probability grid map by using the latest N frames of laser frames at the current moment in the way of ray tracing according to the judgment result.

The device in this embodiment and the method in the previous embodiment are based on two aspects under the same inventive concept. The implementation process of the method has been described in detail above, so those skilled in the art can clearly understand the present invention based on the foregoing description. The structure and implementation process of the system being implemented will not be repeated here for the sake of brevity.

For the convenience of description, when describing the above devices, functions are divided into various modules and described separately. Of course, when implementing the present invention, the functions of each module can be implemented in one or more pieces of software and/or hardware.

It can be known from the above description of the implementation manners that those skilled in the art can clearly understand that the present invention can be implemented by means of software plus a necessary general hardware platform. Based on this understanding, the essence of the technical solution of the present invention or the part that contributes to the prior art can be embodied in the form of software products, and the computer software products can be stored in storage media, such as ROM/RAM, disk , CD, etc., including several instructions to make a computer device (which may be a personal computer, server, or network device, etc.) execute the methods described in various embodiments or some parts of the embodiments of the present invention.

The described device embodiments are only illustrative, wherein the modules or units described as separate components may or may not be physically separated, and the components illustrated as modules or units may or may not be physical modules, either It can be located in one place, or it can be distributed over multiple network modules. Part or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment. It can be understood and implemented by those skilled in the art without creative effort.

The invention is applicable to numerous general purpose and special purpose computing system environments or configurations. Examples: personal computers, server computers, handheld or portable devices, tablet-type devices, multiprocessor systems, microprocessor-based systems, set-top boxes, programmable consumer electronics, network PCs, minicomputers, mainframe computers, including the above Distributed computing environment of any system or device, etc., such as Embodiment 4.

Embodiment four

The odometer device as shown in the figure includes: a processor and a memory for storing instructions executable by the processor;

The processor is configured to:

Obtain the pose of the motion equipment at the current moment, the second laser frame at the current moment, and the local point cloud map,

According to the pose at the current moment, as well as the second laser frame and the local point cloud map, the optimal pose at the current moment is obtained.

The odometer provided by the embodiment of the present invention solves the inaccurate and accumulated errors of existing odometers. For those skilled in the art, various other corresponding changes and modifications can be made according to the technical solutions and ideas described above, and all these changes and modifications should fall within the protection scope of the claims of the present invention.

Claims (17)

1. A odometer method, characterized in that, comprising Obtain the current pose of the motion device, the second laser frame at the current moment, and the local point cloud map; According to the pose at the current moment, as well as the second laser frame and the local point cloud map, the optimal pose at the current moment is calculated. 2. The odometer method according to claim 1, wherein the pose of the motion equipment at the current moment is used as an initial value, and the point cloud matching of the second laser frame and the local point cloud map is carried out to obtain the optimal pose. 3. The odometer method according to claim 2, characterized in that, the optimal pose is specifically obtained by solving the optimal solution of the following formula, and the formula is: in: is the surface formed by the local point cloud map; p _i is the laser point in the second laser frame; the symbol Represents a rotation-translation transformation; the symbol Surfaces representing points to local point cloud maps The Euclidean projection of ; P is the pose during the calculation process, with the pose at the current moment as the initial value. 4. The odometer method according to claim 1, wherein the second laser frame is a second laser frame after dynamic laser points are removed. 5. odometer method according to claim 4, is characterized in that, the step of rejecting the dynamic laser spot in the second laser frame comprises: Obtain the position coordinates of the laser point in the local probability grid map in the second laser frame according to the pose at the current moment; Determine the grid coordinates of the laser point in the second laser frame on the local probability grid map according to the position coordinates; Determine whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located, and if so, remove the dynamic laser point. 6. The odometer method according to claim 1, characterized in that the pose at the current moment is a better pose, specifically, the acquisition of the better pose comprises the steps of: Obtain the initial relative movement of the sports equipment from the first moment to the current moment; Obtain the first laser frame at the first moment and the second laser frame at the current moment; According to the initial relative motion, and the first laser frame and the second laser frame, the relative motion between frames is obtained; According to the pose of the motion equipment at the first moment and the relative motion between frames, the optimal pose at the current moment is obtained; 7. The odometer method according to claim 6, characterized in that, taking the relative motion amount as an initial value, performing inter-frame matching on the first laser frame and the second laser frame to obtain the inter-frame motion amount. 8. The odometer method according to claim 7, wherein the amount of motion between frames is specifically obtained by solving the optimal solution of the following formula, and the formula is: in: is the curved surface formed by the first laser frame; p _i is the laser point of the second laser frame; q is the relative motion of the moving device during the calculation process, with the relative motion as the initial value; the symbol Represents a rotation-translation transformation; the symbol represents the surface formed from the point to the first laser frame Euclidean projection. 9. The odometer method according to any one of claims 1-8, wherein updating the map according to the optimal pose and the second laser frame includes updating the local point cloud map and/or updating the local probability grid map . 10. The odometer method according to any one of claims 1-8, characterized in that, when the displacement of the relative motion amount reaches a set threshold, the grid map is reset, and the grid map reset includes a reset grid The probability value of the grid map, reset the map origin, and update the local probability grid map. 11. An odometer device, characterized in that it comprises The first acquisition module acquires the pose of the sports equipment at the current moment; The second acquisition module acquires the second laser frame at the current moment of the sports equipment; The third acquisition module acquires the local point cloud map; The first calculation module calculates the optimal pose at the current moment according to the pose at the current moment, as well as the second laser frame and the local point cloud map. 12. The odometer device according to claim 11, characterized in that, the device also includes a rejection module for removing the dynamic laser points in the second laser frame; after the second acquisition module acquires the dynamic laser points removed of the second laser frame. 13. The odometer device according to claim 12, wherein the rejecting module comprises The first coordinate unit obtains the position coordinates of the laser point in the local probability grid map in the second laser frame according to the pose at the current moment; The second coordinate unit determines the grid coordinates of the laser point in the second laser frame on the local probability grid map according to the position coordinates; The judging unit judges whether the laser point is a dynamic laser point according to the probability value of the grid where the grid coordinates are located; The rejecting unit, the judgment result of the judging unit rejects the dynamic laser spot, that is, the judgment result is a dynamic laser spot, and then the dynamic laser spot is rejected. 14. The odometer device according to any one of claims 11-13, characterized in that the device also includes a map update module for updating the local point cloud map and/or according to the optimal pose and the second laser frame Or update a local probability raster map. 15. The odometer device according to any one of claims 11-13, further comprising a map reset module, configured to reset the local probability grid map according to the moving distance of the sports equipment. 16. The odometer device according to any one of claims 11-13, wherein the first acquisition module is specifically used to acquire a better pose at the current moment, including The first acquisition unit acquires the initial relative movement amount of the sports equipment from the first moment to the current moment; The second acquiring unit is configured to acquire the first laser frame at the first moment and the second laser frame at the current moment; The first calculation unit obtains the relative motion between frames according to the initial relative motion, and the first laser frame and the second laser frame; The second calculation unit calculates and obtains the optimal pose at the current moment according to the pose of the moving device at the first moment and the relative movement amount between frames. 17. An odometer device, characterized in that, the odometer device comprises: a processor and a memory for storing instructions executable by the processor; The processor is configured to: Obtain the pose of the motion equipment at the current moment, the second laser frame at the current moment, and the local point cloud map, According to the pose at the current moment, as well as the second laser frame and the local point cloud map, the optimal pose at the current moment is obtained.