CN113552864A - Positioning method and device of self-moving body, self-moving body and storage medium - Google Patents
Positioning method and device of self-moving body, self-moving body and storage medium Download PDFInfo
- Publication number
- CN113552864A CN113552864A CN202010294181.9A CN202010294181A CN113552864A CN 113552864 A CN113552864 A CN 113552864A CN 202010294181 A CN202010294181 A CN 202010294181A CN 113552864 A CN113552864 A CN 113552864A
- Authority
- CN
- China
- Prior art keywords
- laser
- key frame
- self
- information
- moving body
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 238000003860 storage Methods 0.000 title claims abstract description 22
- 230000000007 visual effect Effects 0.000 claims abstract description 134
- 238000004590 computer program Methods 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 230000009471 action Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 241001417527 Pempheridae Species 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 239000013307 optical fiber Substances 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0248—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means in combination with a laser
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The embodiment of the invention discloses a positioning method and a positioning device for a self-moving main body, the self-moving main body and a storage medium, wherein the method comprises the following steps: determining a laser key frame according to laser data information, wherein the laser data information comprises multi-frame laser data; determining first position information of the self-moving body at the current moment according to the laser key frame; determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images; performing visual loop matching according to the visual key frame to determine second pose information of the self-moving body at the current moment; and correcting the first position and posture information according to the error between the first position and posture information and the second position and posture information. The embodiment of the invention realizes the real-time positioning of the self-moving body, reduces the accumulated error of the traditional laser positioning through the visual loop matching and improves the positioning accuracy of the self-moving body.
Description
Technical Field
The embodiment of the invention relates to the technical field of automatic positioning, in particular to a positioning method and device of a self-moving main body, the self-moving main body and a storage medium.
Background
An Automatic Guided Vehicle (AGV) is a mobile robot integrating functions of perception, planning, decision making, movement and the like, can guide a line to move according to regulations, is a key carrier device in a modern logistics system, and provides an extremely important support for intelligent, integrated and efficient modern industrial production.
The common AGV equipment adopts a laser sensor positioning scheme, obtains distance information on a horizontal plane through a laser sensor, processes environment information scanned at different moments, extracts similar characteristics for matching to calculate possible pose information of the AGV, and calculates pose information with the highest probability as real-time pose information of the AGV. When the positioning mode is used for scanning environment information, only obstacle distance information located on a horizontal plane with the laser sensor can be obtained, and when the obstacle information on the horizontal plane is highly similar, the map construction and positioning of the AGV generate large errors.
Disclosure of Invention
In view of this, embodiments of the present invention provide a method and an apparatus for positioning a self-moving body, and a storage medium, so as to improve positioning accuracy of the self-moving body when constructing a map and closeness of the map.
In a first aspect, an embodiment of the present invention provides a method for positioning a self-moving body, including:
determining a laser key frame according to laser data information, wherein the laser data information comprises multi-frame laser data;
determining first position information of the self-moving body at the current moment according to the laser key frame;
determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images;
performing visual loop matching according to the visual key frame to determine second pose information of the self-moving body at the current moment;
and correcting the first position and posture information according to the error between the first position and posture information and the second position and posture information.
Further, the determining the laser key frame according to the laser data information includes:
determining whether the current frame laser data of the laser data information meets a first preset condition of a laser key frame;
if the current frame laser data meets a first preset condition of a laser key frame, taking the current frame laser data as the laser key frame;
and if the current frame of laser data does not accord with the first preset condition of the laser key frame, taking the next frame of laser data as the current frame of laser data, and returning to the step of determining whether the current frame of laser data information accords with the first preset condition of the laser key frame.
Further, the determining the visual key frame according to the image information includes:
if the current frame image in the image information meets a second preset condition of a visual key frame, taking the current frame image as the visual key frame;
correspondingly, the performing of the visual loop matching according to the visual key frame to determine the second pose information of the self-moving body at the current time includes:
and performing visual loop matching on the current frame image and the visual key frame to determine second position information of the self-moving body at the current moment.
Further, after determining the first pose information of the self-moving body at the current time according to the laser keyframe, the method further includes: and if the visual key frame cannot be determined according to the image information, performing laser loop matching according to the laser key frame to determine second position information of the self-moving body at the current moment.
Further, the determining the first pose information of the self-moving body at the current time according to the laser key frame includes:
and comparing the current frame laser data with the last frame laser key frame in a scanning matching mode to determine the first position information of the self-moving body at the current moment.
Further, after determining the laser key frame according to the laser data information, the method further includes:
and constructing a map according to the laser key frame.
Further, after performing the visual loop matching to determine the second pose information of the self-moving body at the current time according to the visual key frame, the method further includes:
and correcting the map according to the error between the first position and the second position.
In a second aspect, an embodiment of the present invention provides a positioning apparatus for a self-moving body, including:
the laser key frame determining module is used for determining a laser key frame according to laser data information, wherein the laser data information comprises a plurality of frames of laser data;
the pose information determining module is used for determining first pose information of the self-moving body at the current moment according to the laser key frame;
the visual key frame determining module is used for determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images;
the visual loopback matching module is used for performing visual loopback matching according to the visual key frame to determine second posture information of the self-moving body at the current moment;
and the correction module is used for correcting the first position information according to the error between the first position information and the second position information.
In a third aspect, an embodiment of the present invention provides a self-moving body, including:
one or more processors;
storage means for storing one or more programs;
the laser sensor is used for acquiring laser data information;
an image sensor for acquiring image information;
when the one or more programs are executed by the one or more processors, the one or more processors implement the positioning method of the self-moving body provided by any embodiment of the present invention.
In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements the positioning method for a self-moving body provided in any embodiment of the present invention.
The positioning method of the self-moving body provided by the embodiment of the invention determines the laser key frame according to the laser data information, wherein the laser data information comprises multi-frame laser data; determining first position information of the self-moving body at the current moment according to the laser key frame; determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images; performing visual loop matching according to the visual key frame to determine second pose information of the self-moving body at the current moment; and correcting the first position and posture information according to the error between the first position and posture information and the second position and posture information. The real-time positioning of the self-moving body is realized, the accumulated error of the traditional laser positioning is reduced through the visual loop matching, and the positioning accuracy of the self-moving body is improved.
Drawings
Fig. 1 is a schematic flowchart of a positioning method for a self-moving body according to an embodiment of the present invention;
fig. 2 is a flowchart illustrating a positioning method for a self-moving body according to a second embodiment of the present invention;
fig. 3 is a schematic structural diagram of a positioning device of a self-moving body according to a third embodiment of the present invention;
fig. 4 is a schematic structural diagram of a self-moving body according to a fourth embodiment of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples. It is to be understood that the specific embodiments described herein are merely illustrative of the invention and are not limiting of the invention. It should be further noted that, for the convenience of description, only some of the structures related to the present invention are shown in the drawings, not all of the structures.
Before discussing exemplary embodiments in more detail, it should be noted that some exemplary embodiments are described as processes or methods depicted as flowcharts. Although a flowchart may describe the steps as a sequential process, many of the steps can be performed in parallel, concurrently or simultaneously. In addition, the order of the steps may be rearranged. A process may be terminated when its operations are completed, but may have additional steps not included in the figure. A process may correspond to a method, a function, a procedure, a subroutine, a subprogram, etc.
Furthermore, the terms "first," "second," and the like may be used herein to describe various orientations, actions, steps, elements, or the like, but the orientations, actions, steps, or elements are not limited by these terms. These terms are only used to distinguish one direction, action, step or element from another direction, action, step or element. The terms "first", "second", etc. are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of the present invention, "plurality", "batch" means at least two, e.g., two, three, etc., unless specifically limited otherwise.
Example one
Fig. 1 is a schematic flow chart of a positioning method for a self-moving body according to an embodiment of the present invention, which is applicable to real-time positioning and map building of the self-moving body, where the self-moving body refers to an object capable of moving autonomously, such as a robot, an autonomous vehicle, a sweeper, and the like. As shown in fig. 1, a positioning method of a self-moving body according to an embodiment of the present invention includes:
s110, determining a laser key frame according to laser data information, wherein the laser data information comprises multi-frame laser data.
Specifically, the laser data information is acquired by a laser sensor or a laser radar device provided on the self-moving body, which may be any object capable of autonomous movement, for example, an AGV (Automated Guided Vehicle). One frame of data is a unit in data transmission, the laser data information generally includes multiple frames of laser data, the laser key frames generally are key points of new things appearing in the laser data information, adjacent laser key frames include common points and different points, and the first frame of laser data obtained from the mobile body at the starting pose is generally used as an initial laser key frame.
And S120, determining the first position information of the self-moving body at the current moment according to the laser key frame.
Specifically, after the laser key frame is determined, the actual pose information of the self-moving body at the current moment can be determined by comparing the laser data of the current frame with the laser key frame data of the previous frame, which is called as first pose information. The pose information generally includes displacement and rotation angle, and the current frame laser data is compared with the laser key frame, that is, the relative displacement and relative rotation angle between the current frame laser data and the previous frame laser key frame are determined, so that the first pose information of the self-moving body at the current moment can be determined according to the relative displacement and the relative rotation angle on the basis of the pose information of the previous frame laser key frame where the self-moving body is located.
S130, determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images.
Specifically, the image information is acquired by an image sensor provided on the self-moving body, and the image sensor may use a monocular camera, a binocular camera, a wide-angle camera, or the like. The image information acquired by the image sensor is continuous, and one frame of image is a still image in the image information and is the minimum unit in the image information, so that the image information comprises a plurality of frames of images. The visual key frames are usually key points at which new things appear in image information, and adjacent visual key frames contain both common points and different points, and the first frame of visual image obtained from the moving subject at the start pose is usually taken as an initial visual key frame.
It should be noted that the acquisition of the laser data information and the acquisition of the image information are performed synchronously, and the processing of the laser data information and the processing of the image information are also performed synchronously, and the steps in the embodiment of the present invention are not limited to the order of execution.
And S140, performing visual loop matching according to the visual key frame to determine second position information of the self-moving body at the current moment.
Specifically, the visual loop matching is to compare the currently obtained visual key frame with all the previously stored visual key frames to determine whether the currently obtained visual key frame is one of all the previously stored visual key frames, that is, to determine whether the self-moving object has reached the current pose before. Because the first pose information of the self-moving body at the current moment is obtained according to the relative pose information between the current frame laser data and the last frame laser key frame, when the running path of the self-moving body is longer, the accumulated error is larger, if the self-moving body is determined to reach the current pose through the visual loop matching, the historical key frame obtained when the self-moving body reaches the pose is taken as the currently obtained visual key frame for calculation, and the accumulated error can be reduced. The second pose information of the self-moving body at the current moment refers to pose information of the self-moving body at the current moment obtained by comparing the current frame image with the currently obtained visual key frame.
S150, correcting the first position information according to the error between the first position information and the second position information.
Specifically, the first pose information is obtained by relying on the last laser keyframe, the second pose information is obtained by visual loop matching and accumulated errors are reduced, errors between the second pose information and the first pose information are accumulated errors, the first pose information is corrected according to the accumulated errors, actual pose information of the self-moving body at the current moment is corrected, and therefore the self-moving body is more accurately positioned.
The positioning method of the self-moving body provided by the embodiment of the invention determines the laser key frame according to the laser data information, wherein the laser data information comprises multi-frame laser data; determining first position information of the self-moving body at the current moment according to the laser key frame; determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images; performing visual loop matching according to the visual key frame to determine second pose information of the self-moving body at the current moment; and correcting the first position and posture information according to the error between the first position and posture information and the second position and posture information. The real-time positioning of the self-moving body is realized, the accumulated error of the traditional laser positioning is reduced through the visual loop matching, and the positioning accuracy of the self-moving body is improved.
Example two
Fig. 2 is a schematic flow chart of a positioning method of a self-moving body according to a second embodiment of the present invention, which further details the above-described embodiment. As shown in fig. 2, a positioning method of a self-moving body according to a second embodiment of the present invention includes:
s210, determining whether the laser data of the current frame of the laser data information meets a first preset condition of a laser key frame.
Specifically, the laser data information is acquired by a laser sensor or a laser radar device provided on the self-moving body, which may be any object capable of autonomous movement, for example, an AGV (Automated Guided Vehicle). One frame of data is a unit in data transmission, the laser data information generally includes multiple frames of laser data, the laser key frames generally are key points of new things appearing in the laser data information, adjacent laser key frames include common points and different points, and the first frame of laser data obtained from the mobile body at the starting pose is generally used as an initial laser key frame. The first preset condition whether the current frame laser data meets the laser key frame or not means whether the current frame laser data and the previous frame laser key frame have a certain proportion of similarity, for example, 60% -70% of similarity.
And S220, if the current frame laser data meets a first preset condition of a laser key frame, taking the current frame laser data as the laser key frame.
Specifically, the fact that the current frame of laser data conforms to the first preset condition of the laser key frame means that if the current frame of laser data has a certain proportion of similarity with the previous frame of laser key frame, the current frame of laser data can be used as the laser key frame. The certain ratio includes a ratio range, for example, if the laser data of the current frame has a similarity of 60% to 70% with the laser key frame of the previous frame, the laser data of the current frame can be used as the laser key.
And S230, if the current frame of laser data does not meet the first preset condition of the laser key frame, taking the next frame of laser data as the current frame of laser data, and returning to the step S210.
Specifically, there are two situations when the current frame laser data does not meet the first preset condition of the laser key frame: the similarity between the laser data of the current frame and the laser key frame of the previous frame is too high, and the similarity between the laser data of the current frame and the laser key frame of the previous frame is too low. When the similarity between the laser data of the current frame and the laser key frame of the previous frame is too high, the self-moving body is possibly in a static state; when the similarity between the laser data of the current frame and the laser key frame of the previous frame is too low, it indicates that the laser data acquired from the mobile body may be incorrect. At this time, the similarity between the next frame of laser data and the previous frame of laser key frame should be continuously compared, that is, the next frame of laser data is taken as the current frame of laser data, and the step S210 is returned to extract the laser key frame from the subsequent multiple frames of laser data.
S240, comparing the laser data of the current frame with the laser key frame of the previous frame in a scanning matching mode to determine first position information of the self-moving body at the current moment.
Specifically, after the laser key frame is determined, the actual pose information of the self-moving body at the current moment, which is called first pose information, can be determined by comparing the laser data of the current frame with the laser key frame of the previous frame, and the pose information generally includes displacement and a rotation angle. Through a scanning matching mode, the relative displacement and the relative rotation angle of the current frame laser data relative to the last frame laser key frame can determine the first position information of the self-moving body at the current moment according to the relative displacement and the relative rotation angle on the basis of the position information of the last frame laser key frame where the self-moving body is located.
And S250, constructing a map according to the laser key frame.
Specifically, the positioning of the self-moving body is usually to determine the pose of the self-moving body in the environment where the self-moving body is located, so that a map of the environment where the self-moving body is located needs to be constructed during the positioning.
And S260, if the current frame image in the image information meets a second preset condition of the visual key frame, taking the current frame image as the visual key frame.
Specifically, the image information is acquired by an image sensor provided on the self-moving body, and the image sensor may use a monocular camera, a binocular camera, a wide-angle camera, or the like. The image information acquired by the image sensor is continuous, and one frame of image is a still image in the image information and is the minimum unit in the image information, so that the image information comprises a plurality of frames of images. The visual key frames are usually key points at which new things appear in image information, and adjacent visual key frames contain both common points and different points, and the first frame of visual image obtained from the moving subject at the start pose is usually taken as an initial visual key frame.
Further, the second preset condition of the visual key frame may be that the current frame image and the previous frame visual key frame have a similarity in a certain proportion range, or that the current frame image and the previous frame visual key frame have a certain number of same image features. For example, if the current frame image has a similarity of 60% to 70% with the previous frame of the visual key frame, the current frame image is taken as the visual key frame. If the similarity between the current image and the last frame of the visual key frame is too high, the self-moving body is possibly in a static state; if the similarity between the current frame image and the previous frame of the visual key frame is too low, it indicates that the image information obtained from the mobile subject may be incorrect. At this time, the next frame image may be used as the current frame image and it is determined whether it meets the preset condition of the visual key frame, so as to extract the visual key frame from the subsequent multi-frame images.
It should be noted that the acquisition of the laser data information and the acquisition of the image information are performed synchronously, and the processing of the laser data information and the processing of the image information are also performed synchronously, and the steps in the embodiment of the present invention are not limited to the order of execution.
And S270, performing visual loop matching on the current frame image and the visual key frame to determine second position information of the self-moving body at the current moment.
Specifically, the visual loop matching is to compare the currently obtained visual key frame with all previously stored visual key frames to determine whether there is a historical visual key frame with a higher similarity to the currently obtained visual key frame in all previously stored historical visual key frames, that is, to determine whether the self-moving body has reached the current pose before. If the similarity between the current obtained visual key frame and one of the historical visual key frames in all the previously stored visual key frames is higher, it is indicated that a loop is detected, and when the positioning calculation is performed, the historical visual key frame and the current obtained visual key frame are calculated respectively, so that the calculation error can be reduced. And if the currently obtained visual key frame is different from any one of all the previously stored visual key frames, indicating that the self-moving body possibly runs to a new pose, and storing the currently obtained visual key frame.
Because the first pose information of the self-moving body at the current moment is obtained according to the relative pose information between the current frame laser data and the last frame laser key frame, when the running path of the self-moving body is longer, the accumulated error is larger, if the self-moving body is determined to reach the current pose by visual loop matching, the historical key frame obtained when the self-moving body reaches the pose and the currently obtained visual key frame are respectively calculated, and the pose information obtained by calculation is corrected according to the error between the historical key frame and the currently obtained visual key frame, so that the accumulated error can be reduced.
The second pose information of the self-moving body at the current moment refers to pose information of the self-moving body at the current moment obtained by comparing the current frame image with the currently obtained visual key frame.
In an alternative embodiment, steps S260 to S270 may also be: and if the visual key frame cannot be determined according to the image information, performing laser loop matching according to the laser key frame to determine second position information of the self-moving body at the current moment.
Specifically, the fact that the visual key frame cannot be determined according to the image information means that the image information is failed to be acquired, or the current frame image does not have the similarity with the previous frame visual key frame in a certain proportion range all the time. If an image sensor on the self-moving body is damaged, the determination of the visual key frame cannot be performed, and the visual loop matching cannot be performed, and at this time, the laser loop matching can be performed through the laser key frame to determine the second position posture information of the self-moving body at the current moment, wherein the principle of the laser loop matching is the same as that of the visual loop matching, and is not repeated herein, the difference between the two is that the processed data is different, the laser loop matching uses the laser key frame for loop matching, and the visual loop matching uses the visual key frame for loop matching.
S280, correcting the first position information according to the error between the first position information and the second position information.
Specifically, the first pose information is obtained by relying on the last laser keyframe, the second pose information is obtained by visual loop matching and accumulated errors are reduced, errors between the second pose information and the first pose information are accumulated errors, the first pose information is corrected according to the accumulated errors, actual pose information of the self-moving body at the current moment is corrected, and therefore the self-moving body is more accurately positioned.
And S290, correcting the map according to the error between the first position and the second position.
Specifically, the constructed map is corrected through accumulated errors obtained through loop matching, so that the map data are more accurate. For example, the map constructed is rotated by 5 ° as a whole and translated by 0.5cm in the advancing direction of the self-moving body according to the accumulated error, so that the orientation of the map is more accurate, and then the orientation of the self-moving body is also more accurate. For another example, if the current pose is detected by loop matching to be a pose that has been reached before by the moving subject, the point is marked, so that the constructed map has good closeness.
The positioning method of the self-moving body provided by the embodiment of the invention realizes the real-time positioning of the self-moving body through the combination of the laser data information and the image data information, reduces the accumulated error of the traditional laser positioning through the visual loop matching, and improves the positioning accuracy of the self-moving body and the closeness of a map.
EXAMPLE III
Fig. 3 is a schematic structural diagram of a positioning apparatus for a self-moving body according to a third embodiment of the present invention, which is applicable to real-time positioning and map building of the self-moving body, where the self-moving body refers to an object capable of moving autonomously, such as a robot, an autonomous vehicle, a sweeper, and the like. The positioning device of the self-moving body provided by the third embodiment of the present invention can implement the positioning method of the self-moving body provided by any embodiment of the present invention, and has corresponding functional structures and beneficial effects of the implementation method.
As shown in fig. 3, a positioning device of a self-moving body according to a third embodiment of the present invention includes: laser key frame determination module 310, pose information determination module 320, visual key frame determination module 330, and visual loop matching module 340 and rework module 350.
The laser key frame determining module 310 is configured to determine a laser key frame according to laser data information, where the laser data information includes multiple frames of laser data;
the pose information determining module 320 is configured to determine first pose information of the self-moving body at the current time according to the laser key frame;
the visual key frame determining module 330 is configured to determine a visual key frame according to image information, where the image information includes multiple frames of images;
the visual loop matching module 340 is configured to perform visual loop matching according to the visual keyframe to determine second pose information of the self-moving body at the current time;
the correcting module 350 is configured to correct the first pose information according to an error between the first pose information and the second pose information.
Further, the laser key frame determination module 310 is specifically configured to:
determining whether the current frame laser data of the laser data information meets a first preset condition of a laser key frame;
if the current frame laser data meets a first preset condition of a laser key frame, taking the current frame laser data as the laser key frame;
and if the current frame of laser data does not accord with the first preset condition of the laser key frame, taking the next frame of laser data as the current frame of laser data, and returning to the step of determining whether the current frame of laser data information accords with the first preset condition of the laser key frame.
Further, the visual key frame determination module 330 is specifically configured to: if the current frame image in the image information meets the preset condition of the visual key frame, taking the current frame image as the visual key frame; accordingly, the visual loop matching module 340 is specifically configured to: and performing visual loop matching on the current frame image and the visual key frame to determine second position information of the self-moving body at the current moment.
Further, the method also comprises the following steps: and the laser loop matching module is used for performing laser loop matching according to the laser key frame to determine second position and posture information of the self-moving body at the current moment if the visual key frame cannot be determined according to the image information.
Further, the pose information determining module 320 is specifically configured to: and comparing the current frame laser data with the last frame laser key frame in a scanning matching mode to determine the first position information of the self-moving body at the current moment.
Further, the method also comprises the following steps: and the map building module is used for building a map according to the laser key frame.
Further, the modification module 350 is further configured to: and correcting the map according to the error between the first position and the second position.
The positioning device of the self-moving body provided by the third embodiment of the invention realizes the real-time positioning of the self-moving body through the laser key frame determining module, the pose information determining module, the visual key frame determining module, the visual loop matching module and the correcting module, reduces the accumulated error of the traditional laser positioning through the visual loop matching, and improves the positioning accuracy of the self-moving body and the closeness of a map.
Example four
Fig. 4 is a schematic structural diagram of a self-moving body according to a fourth embodiment of the present invention, in this embodiment, the self-moving body refers to an object capable of moving autonomously, for example, a robot, an autonomous vehicle, a sweeper, and the like. As shown in fig. 4, the self-moving body includes a processor 410, a memory 420, an input device 430, an output device 440, a laser sensor 450, and an image sensor 460; the number of processors 410 in the device/terminal/server may be one or more, and one processor 410 is taken as an example in fig. 4; the processor 410, the memory 420, the input device 430 and the output device 440 in the device/terminal/server may be connected by a bus or other means, as exemplified by the bus connection in fig. 4.
The memory 420 serves as a computer-readable storage medium for storing software programs, computer-executable programs, and modules, such as program instructions/modules corresponding to the positioning method of the self-moving body in the embodiment of the present invention (for example, the laser key frame determination module 310, the pose information determination module 320, the visual key frame determination module 330, and the visual loop matching module 340 and the correction module 350 in the positioning apparatus of the self-moving body). The processor 410 executes various functional applications of the device/terminal/server and data processing by executing software programs, instructions and modules stored in the memory 420, that is, the above-mentioned positioning method of the self-moving body is realized, and the method may include: determining a laser key frame according to laser data information, wherein the laser data information comprises multi-frame laser data; determining first position information of the self-moving body at the current moment according to the laser key frame; determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images; performing visual loop matching according to the visual key frame to determine second pose information of the self-moving body at the current moment; and correcting the first position and posture information according to the error between the first position and posture information and the second position and posture information.
The memory 420 may mainly include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application program required for at least one function; the storage data area may store data created according to the use of the terminal, and the like. Further, the memory 420 may include high speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other non-volatile solid state storage device. In some examples, the memory 420 may further include memory located remotely from the processor 410, which may be connected to the device/terminal/server via a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
The input device 430 may be used to receive input numeric or character information and generate key signal inputs related to user settings and function control from the mobile body. The output device 440 may include a display device such as a display screen.
The laser sensor 450 is used to obtain laser data information, and may be a common laser sensor, a laser radar sensor, or the like. The image sensor 460 is used to acquire image information and may be a monocular camera, a binocular camera, a wide-angle camera, or the like.
EXAMPLE five
An embodiment of the present invention further provides a computer-readable storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements a positioning method for a self-moving body according to any embodiment of the present invention, where the method may include: determining a laser key frame according to laser data information, wherein the laser data information comprises multi-frame laser data; determining first position information of the self-moving body at the current moment according to the laser key frame; determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images; performing visual loop matching according to the visual key frame to determine second pose information of the self-moving body at the current moment; and correcting the first position and posture information according to the error between the first position and posture information and the second position and posture information.
Computer storage media for embodiments of the invention may employ any combination of one or more computer-readable media. The computer readable medium may be a computer readable signal medium or a computer readable storage medium. A computer readable storage medium may be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination of the foregoing. More specific examples (a non-exhaustive list) of the computer readable storage medium would include the following: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing. In the context of this document, a computer readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device.
A computer readable signal medium may include a propagated data signal with computer readable program code embodied therein, for example, in baseband or as part of a carrier wave. Such a propagated data signal may take many forms, including, but not limited to, electro-magnetic, optical, or any suitable combination thereof. A computer readable signal medium may also be any computer readable medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device.
Program code embodied on a computer readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.
Computer program code for carrying out operations for aspects of the present invention may be written in any combination of one or more programming languages, including an object oriented programming language such as Java, Smalltalk, C + + or the like and conventional procedural programming languages, such as the "C" programming language or similar programming languages. The program code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or terminal. In the case of a remote computer, the remote computer may be connected to the user's computer through any type of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or the connection may be made to an external computer (for example, through the Internet using an Internet service provider).
It is to be noted that the foregoing is only illustrative of the preferred embodiments of the present invention and the technical principles employed. It will be understood by those skilled in the art that the present invention is not limited to the particular embodiments described herein, but is capable of various obvious changes, rearrangements and substitutions as will now become apparent to those skilled in the art without departing from the scope of the invention. Therefore, although the present invention has been described in greater detail by the above embodiments, the present invention is not limited to the above embodiments, and may include other equivalent embodiments without departing from the spirit of the present invention, and the scope of the present invention is determined by the scope of the appended claims.
Claims (10)
1. A method of locating a self-moving body, comprising:
determining a laser key frame according to laser data information, wherein the laser data information comprises multi-frame laser data;
determining first position information of the self-moving body at the current moment according to the laser key frame;
determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images;
performing visual loop matching according to the visual key frame to determine second pose information of the self-moving body at the current moment;
and correcting the first position and posture information according to the error between the first position and posture information and the second position and posture information.
2. The method of claim 1, wherein determining a laser keyframe from laser data information comprises:
determining whether the current frame laser data of the laser data information meets a first preset condition of a laser key frame;
if the current frame laser data meets a first preset condition of a laser key frame, taking the current frame laser data as the laser key frame;
and if the current frame of laser data does not accord with the first preset condition of the laser key frame, taking the next frame of laser data as the current frame of laser data, and returning to the step of determining whether the current frame of laser data information accords with the first preset condition of the laser key frame.
3. The method of claim 1, wherein determining a visual key frame from image information comprises:
if the current frame image in the image information meets a second preset condition of a visual key frame, taking the current frame image as the visual key frame;
correspondingly, the performing of the visual loop matching according to the visual key frame to determine the second pose information of the self-moving body at the current time includes:
and performing visual loop matching on the current frame image and the visual key frame to determine second position information of the self-moving body at the current moment.
4. The method of claim 1, wherein after determining the first pose information for the self-moving body at the current time from the laser keyframe, further comprising:
and if the visual key frame cannot be determined according to the image information, performing laser loop matching according to the laser key frame to determine second position information of the self-moving body at the current moment.
5. The method of claim 1, wherein determining first pose information for the self-moving body at the current time from the laser keyframe comprises:
and comparing the current frame laser data with the last frame laser key frame in a scanning matching mode to determine the first position information of the self-moving body at the current moment.
6. The method of claim 1, wherein after determining the laser key frame based on the laser data information, further comprising:
and constructing a map according to the laser key frame.
7. The method of claim 6, wherein after performing visual loop matching from the visual keyframes to determine second pose information of the self-moving body at the current time, further comprising:
and correcting the map according to the error between the first position and the second position.
8. A positioning device for a self-moving body, comprising:
the laser key frame determining module is used for determining a laser key frame according to laser data information, wherein the laser data information comprises a plurality of frames of laser data;
the pose information determining module is used for determining first pose information of the self-moving body at the current moment according to the laser key frame;
the visual key frame determining module is used for determining a visual key frame according to image information, wherein the image information comprises a plurality of frames of images;
the visual loopback matching module is used for performing visual loopback matching according to the visual key frame to determine second posture information of the self-moving body at the current moment;
and the correction module is used for correcting the first position information according to the error between the first position information and the second position information.
9. A self-moving body, comprising:
one or more processors;
storage means for storing one or more programs;
the laser sensor is used for acquiring laser data information;
an image sensor for acquiring image information;
when executed by the one or more processors, cause the one or more processors to implement the method of positioning a self-moving body as recited in any of claims 1-7.
10. A computer-readable storage medium, on which a computer program is stored, which, when being executed by a processor, carries out a method of positioning a self-moving body according to any one of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010294181.9A CN113552864A (en) | 2020-04-15 | 2020-04-15 | Positioning method and device of self-moving body, self-moving body and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010294181.9A CN113552864A (en) | 2020-04-15 | 2020-04-15 | Positioning method and device of self-moving body, self-moving body and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113552864A true CN113552864A (en) | 2021-10-26 |
Family
ID=78100911
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010294181.9A Pending CN113552864A (en) | 2020-04-15 | 2020-04-15 | Positioning method and device of self-moving body, self-moving body and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113552864A (en) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105856230A (en) * | 2016-05-06 | 2016-08-17 | 简燕梅 | ORB key frame closed-loop detection SLAM method capable of improving consistency of position and pose of robot |
| CN106153048A (en) * | 2016-08-11 | 2016-11-23 | 广东技术师范学院 | A kind of robot chamber inner position based on multisensor and Mapping System |
| CN106780601A (en) * | 2016-12-01 | 2017-05-31 | 北京未动科技有限公司 | A kind of locus method for tracing, device and smart machine |
| CN109141395A (en) * | 2018-07-10 | 2019-01-04 | 深圳市沃特沃德股份有限公司 | A kind of the sweeper localization method and device of view-based access control model winding calibration gyroscope |
| CN109752724A (en) * | 2018-12-26 | 2019-05-14 | 珠海市众创芯慧科技有限公司 | A kind of image laser integral type navigation positioning system |
| CN109887029A (en) * | 2019-01-17 | 2019-06-14 | 江苏大学 | A kind of monocular vision mileage measurement method based on color of image feature |
| CN110587597A (en) * | 2019-08-01 | 2019-12-20 | 深圳市银星智能科技股份有限公司 | SLAM closed loop detection method and detection system based on laser radar |
-
2020
- 2020-04-15 CN CN202010294181.9A patent/CN113552864A/en active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105856230A (en) * | 2016-05-06 | 2016-08-17 | 简燕梅 | ORB key frame closed-loop detection SLAM method capable of improving consistency of position and pose of robot |
| CN106153048A (en) * | 2016-08-11 | 2016-11-23 | 广东技术师范学院 | A kind of robot chamber inner position based on multisensor and Mapping System |
| CN106780601A (en) * | 2016-12-01 | 2017-05-31 | 北京未动科技有限公司 | A kind of locus method for tracing, device and smart machine |
| CN109141395A (en) * | 2018-07-10 | 2019-01-04 | 深圳市沃特沃德股份有限公司 | A kind of the sweeper localization method and device of view-based access control model winding calibration gyroscope |
| CN109752724A (en) * | 2018-12-26 | 2019-05-14 | 珠海市众创芯慧科技有限公司 | A kind of image laser integral type navigation positioning system |
| CN109887029A (en) * | 2019-01-17 | 2019-06-14 | 江苏大学 | A kind of monocular vision mileage measurement method based on color of image feature |
| CN110587597A (en) * | 2019-08-01 | 2019-12-20 | 深圳市银星智能科技股份有限公司 | SLAM closed loop detection method and detection system based on laser radar |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111442722B (en) | Positioning method, positioning device, storage medium and electronic equipment | |
| CN107481292B (en) | Attitude error estimation method and device for vehicle-mounted camera | |
| US20220371602A1 (en) | Vehicle positioning method, apparatus, and controller, intelligent vehicle, and system | |
| CN111209978B (en) | Three-dimensional visual repositioning method and device, computing equipment and storage medium | |
| EP3671121A1 (en) | Map creation method for mobile robot and path planning method based on map | |
| CN107167826B (en) | Vehicle longitudinal positioning system and method based on variable grid image feature detection in automatic driving | |
| US20220198688A1 (en) | Laser coarse registration method, device, mobile terminal and storage medium | |
| CN111680747B (en) | Method and apparatus for closed loop detection of occupancy grid subgraphs | |
| CN110900602B (en) | Positioning recovery method and device, robot and storage medium | |
| CN114111774B (en) | Vehicle positioning method, system, equipment and computer readable storage medium | |
| CN110288708B (en) | Map construction method and device and terminal equipment | |
| US20230087467A1 (en) | Methods and systems for modeling poor texture tunnels based on vision-lidar coupling | |
| CN112285739B (en) | Data processing method, device, equipment and storage medium | |
| CN114279434B (en) | Picture construction method and device, electronic equipment and storage medium | |
| CN115272452A (en) | Target detection positioning method and device, unmanned aerial vehicle and storage medium | |
| CN116958452A (en) | Three-dimensional reconstruction method and system | |
| CN115638788B (en) | Semantic vector map construction method, computer equipment and storage medium | |
| CN113552864A (en) | Positioning method and device of self-moving body, self-moving body and storage medium | |
| CN116358525A (en) | Laser radar-based map building and positioning method, system and engineering vehicle | |
| CN110244710B (en) | Automatic tracing method, device, storage medium and electronic equipment | |
| CN113947769A (en) | Traffic light detection method and device based on long-focus and short-focus double cameras and storage medium | |
| CN114610035A (en) | Pile returning method and device and mowing robot | |
| CN112906665A (en) | Traffic marking fusion method and device, storage medium and electronic equipment | |
| CN109959935B (en) | Map establishing method, map establishing device and robot | |
| CN115512242B (en) | Scene change detection method and flight device |
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 |