CN111915680A - Robot positioning method, system, electronic device and computer readable medium - Google Patents

Robot positioning method, system, electronic device and computer readable medium Download PDF

Info

Publication number
CN111915680A
CN111915680A CN202010905389.XA CN202010905389A CN111915680A CN 111915680 A CN111915680 A CN 111915680A CN 202010905389 A CN202010905389 A CN 202010905389A CN 111915680 A CN111915680 A CN 111915680A
Authority
CN
China
Prior art keywords
robot
map
dimensional
key frame
camera
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010905389.XA
Other languages
Chinese (zh)
Other versions
CN111915680B (en
Inventor
张腾
高明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Inspur Intelligent IoT Technology Co Ltd
Original Assignee
Shandong New Generation Information Industry Technology Research Institute Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shandong New Generation Information Industry Technology Research Institute Co Ltd filed Critical Shandong New Generation Information Industry Technology Research Institute Co Ltd
Priority to CN202010905389.XA priority Critical patent/CN111915680B/en
Publication of CN111915680A publication Critical patent/CN111915680A/en
Application granted granted Critical
Publication of CN111915680B publication Critical patent/CN111915680B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06K—GRAPHICAL DATA READING; PRESENTATION OF DATA; RECORD CARRIERS; HANDLING RECORD CARRIERS
    • G06K17/00—Methods or arrangements for effecting co-operative working between equipments covered by two or more of main groups G06K1/00 - G06K15/00, e.g. automatic card files incorporating conveying and reading operations
    • G06K17/0022—Methods or arrangements for effecting co-operative working between equipments covered by two or more of main groups G06K1/00 - G06K15/00, e.g. automatic card files incorporating conveying and reading operations arrangements or provisions for transferring data to distant stations, e.g. from a sensing device
    • G06K17/0025—Methods or arrangements for effecting co-operative working between equipments covered by two or more of main groups G06K1/00 - G06K15/00, e.g. automatic card files incorporating conveying and reading operations arrangements or provisions for transferring data to distant stations, e.g. from a sensing device the arrangement consisting of a wireless interrogation device in combination with a device for optically marking the record carrier
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Databases & Information Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Image Analysis (AREA)

Abstract

The invention discloses a robot positioning method, a system, an electronic device and a computer readable medium, belongs to the technical field of robot positioning, and aims to solve the technical problem of how to perform real-time visual positioning on a micro robot in a simple environment. The method comprises the following steps: setting a two-dimensional code on the installation pose of the robot; shooting the two-dimensional code through a plurality of cameras fixed outside to obtain an RGB image; constructing a dense point cloud map, and converting the dense point cloud map into a two-dimensional occupation grid map; calculating the pose of the robot in a two-dimensional occupancy grid map through an ORB _ SALM2 algorithm; converting the key frame information into an internal map based on an ORB _ SALM2 algorithm; and optimizing the internal map through a global clustering optimization thread on the basis of the principle that the moving track of the robot forms a closed loop based on the key frame height matching. The system executes the method for robot positioning.

Description

Robot positioning method, system, electronic device and computer readable medium
Technical Field
The invention relates to the technical field of robot positioning, in particular to a robot positioning method, a robot positioning system, an electronic device and a computer readable medium.
Background
As the research of robots has been advanced, the classification of robots has been refined, and the miniaturization of robots is one of the great trends in the development of robots, but the size, the power, and the cost of accessories have been an obstacle to the miniaturization of robots. The current algorithm for popular visual localization is ORB _ SLAM2, and ORB _ SLAM2 is a three-dimensional localization and mapping algorithm based on ORB features. The algorithm was published in IEEE Transactions on Robotics by Raul Mur-Artal, J.M.M.Montiel and Juan D.Tardos in 2015. The ORB _ SLAM2 is based on a PTAM framework, the functions of map initialization and closed loop detection are added, the methods of key frame selection and map construction are optimized, and good effects are achieved in processing speed, tracking effect and map precision. However, when the ORB _ SLAM2 algorithm is operated, the robot is required to provide images in real time through a camera, and high requirements are placed on the computing power of the computing unit. Experiments show that the robot computing power using the raspberry pi as a computing unit cannot meet the requirement of ORB _ SLAM2 for real-time positioning. Therefore, a robot using the onboard camera plus ORB SLAM2 solution may not have the capability of real-time visual positioning. According to the conclusion, under a simple environment, the low-cost micro robot can solve the problem by adopting a mode of fixing multiple cameras and vision SLAM.
How to carry out real-time visual positioning on the micro-robot in a simple environment is a technical problem to be solved.
Disclosure of Invention
The technical task of the present invention is to provide a robot positioning method, system, electronic device and computer readable medium to solve the technical problem of how to perform real-time visual positioning on a micro-robot in a simple environment.
In a first aspect, the present invention provides a robot positioning method for positioning a robot based on a plurality of cameras fixed outside and a visual SLAM, the method comprising the steps of:
registering the robot and setting a two-dimensional code on the installation pose of the robot, wherein the two-dimensional code contains registration information, is used as a unique identification code of the robot and is used as a pose identification of the robot;
shooting the two-dimensional code through a plurality of cameras fixed outside to obtain an RGB image;
in the tracking thread, extracting key frames and corresponding depth frames from the RGB image based on an ORB algorithm, and calculating key frame data, wherein the key frame data comprises but is not limited to key frame position and posture data;
in a two-dimensional occupation grid map generation thread, constructing a dense point cloud map based on a key frame, a depth frame corresponding to the key frame and key frame position and posture data, and converting the dense point cloud map into a two-dimensional occupation grid map;
calculating the pose of the robot through a binocular camera parallax principle, and calculating the pose of the robot in a two-dimensional occupation grid map through an ORB _ SALM2 algorithm;
in a local map building thread, converting the key frame information into an internal map based on an ORB _ SALM2 algorithm, wherein the internal map comprises movement data of all key frames, and the movement data comprises but is not limited to a movement track and a movement speed;
and in the closed-loop detection thread, detecting whether the key frames are highly matched, and optimizing the internal map through the global cluster optimization thread on the basis of the principle that the moving track of the robot forms a closed loop when the key frames are highly matched.
Preferably, the method for converting the dense point cloud map into the two-dimensional occupancy grid map comprises the following steps:
generating an octree map through the dense point cloud map;
mapping the octree map to 2D generates a two-dimensional occupancy grid map.
Preferably, the robot pose is calculated by a binocular camera parallax principle, and the method comprises the following steps:
setting two cameras as a left camera and a right camera respectively, wherein the two cameras view the same characteristic point P of a space object at the same moment, and the coordinate of the characteristic point P is (x)c,yc,zc) And the image coordinate of the characteristic point P acquired by the left camera is Pleft＝(Xleft,Yleft) And the image coordinate of the characteristic point P acquired by the right camera is Pright＝(Xright,Yright)；
The images of the two cameras are on the same horizontal plane, and the image coordinates Y coordinates of the characteristic points P are the same, namely Yleft＝YrightY, derived from the trigonometric relationship:
Figure BDA0002661238910000031
Figure BDA0002661238910000032
Figure BDA0002661238910000033
Disparity＝Xleftxright, based on the above-mentioned three-dimensional coordinates of the computed feature points P in the camera coordinate system:
Figure BDA0002661238910000034
Figure BDA0002661238910000035
Figure BDA0002661238910000036
wherein, B represents the distance between the projection center connecting lines of the two cameras;
f denotes a camera focal length.
In a second aspect, the present invention provides a robot positioning system, configured to position a robot by using the robot positioning method according to any one of the first aspect, where a two-dimensional code is set in an installation pose of the robot, and the two-dimensional code includes registration information, and is used as a unique identification code of the robot and a pose identifier of the robot; the system comprises:
a registration module to register the robot;
the camera module comprises a camera fixed outside the robot, and the camera is used for shooting the two-dimensional code to obtain an RGB image;
the tracking module is added with a tracking thread, in the tracking thread, key frames and corresponding depth frames are extracted from the RGB images based on an ORB algorithm, and key frame data are calculated, wherein the key frame data comprise but are not limited to key frame position and posture data;
the first map generation module is added with a two-dimensional occupation grid map generation thread, in the two-dimensional occupation grid map generation thread, a dense point cloud map is constructed based on a key frame, a depth frame corresponding to the key frame and key frame position and posture data, and the dense point cloud map is converted into the two-dimensional occupation grid map;
the robot pose calculation module is used for calculating the pose of the robot through the binocular camera parallax principle and calculating the pose of the robot in the two-dimensional occupancy grid map through an ORB _ SALM2 algorithm;
a second map building module, which is added with a local map building thread, and in the local map building thread, the key frame information is converted into an internal map based on an ORB _ SALM2 algorithm, the internal map includes movement data of all key frames, and the movement data includes, but is not limited to, a movement track and a movement speed;
and the optimization module is added with a closed-loop detection thread and a full-set cluster optimization thread, detects whether the key frames are highly matched in the closed-loop detection thread, and optimizes the internal map through the global cluster optimization thread on the basis of the principle that the moving track of the robot forms a closed loop when the key frames are highly matched.
Preferably, the first map generation module is configured to convert the dense point cloud map into a two-dimensional occupancy grid map by:
generating an octree map through the dense point cloud map;
mapping the octree map to 2D generates a two-dimensional occupancy grid map.
Preferably, the robot pose calculation module is configured to calculate the robot pose by:
setting two cameras as a left camera and a right camera respectively, wherein the two cameras view the same characteristic point P of a space object at the same moment, and the coordinate of the characteristic point P is (x)c,yc,zc) And the image coordinate of the characteristic point P acquired by the left camera is Pleft＝(Xleft,Yleft) And the image coordinate of the characteristic point P acquired by the right camera is Pright＝(Xright,Yright)；
The images of the two cameras are on the same horizontal plane, and the image coordinates Y coordinates of the characteristic points P are the same, namely Yleft＝YrightY, derived from the trigonometric relationship:
Figure BDA0002661238910000041
Figure BDA0002661238910000042
Figure BDA0002661238910000043
Disparity＝Xleftxright, based on the above-mentioned three-dimensional coordinates of the computed feature points P in the camera coordinate system:
Figure BDA0002661238910000051
Figure BDA0002661238910000052
Figure BDA0002661238910000053
wherein, B represents the distance between the projection center connecting lines of the two cameras;
f denotes a camera focal length.
In a third aspect, the present invention provides an electronic device, comprising: at least one memory and at least one processor;
the at least one memory to store a machine readable program;
the at least one processor is configured to invoke the machine-readable program to perform the method of any of the first aspects.
In a fourth aspect, the present invention provides a computer readable medium having stored thereon computer instructions which, when executed by a processor, cause the processor to perform the method of any of the first aspects.
The robot positioning method, the system, the electronic device and the computer readable medium have the following advantages:
1. the method comprises the steps that two-dimensional codes are arranged on installation poses of the robot, an external fixed camera shoots the two-dimensional codes to obtain RGB images, a two-dimensional occupation grid map is generated based on the RGB images, the poses of the robot are calculated through a binocular camera parallax principle, the poses of the robot in the two-dimensional occupation grid map are calculated through an ORB _ SALM2 algorithm, the robot is located and calculated and moved to a server side, and cost and calculation force of the robot side are reduced;
2. only one two-dimensional code is needed to be installed at the robot end to serve as a pose identifier, installation setting of a camera at the robot end is eliminated, SLAM calculation is also transferred to the server end, the size requirement of the robot end is reduced, and the method is suitable for a miniature robot.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed for the embodiments or the prior art descriptions will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on the drawings without creative efforts.
The invention is further described below with reference to the accompanying drawings.
FIG. 1 is a block flow diagram of a robot positioning method according to embodiment 1;
fig. 2 is a schematic view of the positioning principle of the binocular camera in the robot positioning method according to embodiment 1.
Detailed Description
The present invention is further described in the following with reference to the drawings and the specific embodiments so that those skilled in the art can better understand the present invention and can implement the present invention, but the embodiments are not to be construed as limiting the present invention, and the embodiments and the technical features of the embodiments can be combined with each other without conflict.
It is to be understood that the terms first, second, and the like in the description of the embodiments of the invention are used for distinguishing between the descriptions and not necessarily for describing a sequential or chronological order. The "plurality" in the embodiment of the present invention means two or more.
The embodiment of the invention provides a robot positioning method, a robot positioning system, an electronic device and a computer readable medium, which are used for solving the technical problem of how to perform real-time visual positioning on a micro robot in a simple environment.
Example 1:
the robot positioning method is used for positioning the robot based on a plurality of cameras fixed outside and a visual SLAM. The method comprises the following steps:
s100, registering the robot and setting a two-dimensional code on the installation pose of the robot, wherein the two-dimensional code contains registration information, is used as a unique identification code of the robot and is used as a pose identification of the robot;
s200, shooting the two-dimensional code through a plurality of cameras fixed outside to obtain an RGB image;
s300, in a tracking thread, extracting key frames and corresponding depth frames from the RGB image based on an ORB algorithm, and calculating key frame data, wherein the key frame data comprises but is not limited to key frame position and posture data;
s400, in a two-dimensional occupation grid map generation thread, constructing a dense point cloud map based on a key frame, a depth frame corresponding to the key frame and key frame position and posture data, and converting the dense point cloud map into a two-dimensional occupation grid map;
s500, calculating the pose of the robot through a binocular camera parallax principle, and calculating the pose of the robot in a two-dimensional occupation grid map through an ORB _ SALM2 algorithm;
s600, in a local map building thread, converting the key frame information into an internal map based on an ORB _ SALM2 algorithm, wherein the internal map comprises movement data of all key frames, and the movement data comprises but is not limited to a movement track and a movement speed;
s700, in the closed-loop detection thread, detecting whether the key frames are highly matched, and optimizing the internal map through the global cluster optimization thread on the basis of the principle that the key frames are highly matched and the moving track of the robot forms a closed loop.
In this embodiment, a robot in a space is registered, and a camera acquires a two-dimensional code on an installation pose of the robot to obtain an RGB image, where the RGB image is a matrix defined by three primary colors of red, green, and blue.
The ORB _ SLAM2 algorithm does not provide a map building function for real-time two-dimensional occupancy grid maps, and is added in the present embodiment in the tracking thread, in which key frames and corresponding depth frames are extracted from RGB images based on the ORB algorithm, and key frame data is calculated, including but not limited to key frame pose data that will be key frames when a new key frame is generated.
Based on ORB algorithm, extracting key frames and corresponding depth frames from RGB images, and calculating key frame data, the specific method is as follows: extracting key points of the RGB image through an ORB algorithm to generate key frames, calculating pose transformation between the two key frames through the change of the relative positions of the key points between the adjacent key frames, predicting speed change between the two key frames, outputting the key frame pose transformation and speed change related data, and generating key frame data.
The essence of the key frame is to reduce the amount of calculation of the ORB _ SLAM2 algorithm, and in the tracking thread, the pose of the key frame is calculated by the position change of the key point between two adjacent frames every time a certain condition is reached (new key point is greater than 20 or the number of frame intervals is greater than 20).
The method comprises the steps that a RGB picture and a depth map corresponding to the RGB picture can be generated, and when the relative positions of two adjacent pictures are known, the two point cloud maps can be combined into a large point cloud map. Based on the above, the key frame, the depth frame corresponding to the key frame, and the key frame pose data are spliced into a dense point cloud map. And when a new key frame is generated, splicing the key frame, the depth frame corresponding to the key frame and the key frame position and posture data into a dense point cloud map, generating an octree map through the dense point cloud map, and mapping the octree map to 2D to generate a two-dimensional occupation grid map.
As shown in fig. 2, the robot pose is calculated by the binocular camera parallax principle, which includes the following steps:
(1) setting two cameras as a left camera and a right camera respectively, wherein the two cameras view the same characteristic point P of a space object at the same moment, and the coordinate of the characteristic point P is (x)c,yc,zc) And the image coordinate of the characteristic point P acquired by the left camera is Pleft＝(Xleft,Yleft) And the image coordinate of the characteristic point P acquired by the right camera is Pright＝(Xright,Yright)；
(2) The images of the two cameras are on the same horizontal plane, and the image coordinates Y coordinates of the characteristic points P are the same, namely Yleft＝YrightY, derived from the trigonometric relationship:
Figure BDA0002661238910000081
Figure BDA0002661238910000082
Figure BDA0002661238910000083
Disparity＝Xleftxright, based on the above-mentioned three-dimensional coordinates of the computed feature points P in the camera coordinate system:
Figure BDA0002661238910000084
Figure BDA0002661238910000085
Figure BDA0002661238910000086
wherein, B represents the distance between the projection center connecting lines of the two cameras;
f denotes a camera focal length.
Any point on the left camera image plane can determine the three-dimensional coordinates of the point as long as the corresponding matching point can be found on the right camera image plane. The method is complete point-to-point operation, and all points on an image surface can participate in the operation as long as corresponding matching points exist, so that corresponding three-dimensional coordinates of the points are obtained.
In the local map building thread, the key frame data generated by the tracking thread is converted into an internal map of ORB _ SLAM2 through calculation, and the internal map carries and combines all information sets of data for each key frame, for example, the key frame data only contains the transformation relation between two key frames, and the internal map contains the relative changes of all key frames, and generates the data such as the movement track and the movement speed of the key frames according to the relative changes.
The closed-loop detection thread is an optimization thread, errors are inevitably generated in the two-dimensional occupation grid map generation thread and the local map construction thread, the errors are larger and larger along with accumulation of running time, the closed-loop detection thread compares key frames to detect whether the key frames are highly matched, and if the key frames are highly matched, closed loop (namely the robot runs to the position which is originally run to) is generated, and the whole map is optimized through the global cluster optimization thread according to the characteristic.
The method includes the steps that a two-dimensional code is arranged at an installation pose of the robot, the two-dimensional code is collected through multiple cameras to generate an RGB image, a key frame and a corresponding depth frame are extracted from the RGB image based on an ORB algorithm, key frame data are calculated, a two-dimensional occupation grid map is constructed based on the key frame, the depth frame corresponding to the key frame and key frame pose data, the pose of the robot is calculated through a binocular camera parallax principle, and the pose of the robot in the two-dimensional occupation grid map is calculated through an ORB _ SALM2 algorithm. In the method, the robot end removes the camera and SLAM operation, so that the cost and the volume of the robot end are reduced, and the computing power of the robot end is released.
Example 2:
the robot positioning system of the present invention is configured to position a robot by the robot positioning method disclosed in embodiment 1, and a two-dimensional code is set in an installation pose of the robot, and the two-dimensional code includes registration information, and is used as a unique identification code of the robot and as a pose identification of the robot.
In this embodiment, the robot positioning system includes a registration module, a camera module, a tracking module, a first map generation module, a robot pose calculation module, a second map construction module, and an optimization module, where the registration module is used to register the robot; the camera module comprises a camera fixed outside the robot, and the camera is used for shooting the two-dimensional code to obtain an RGB image; the tracking module is added with a tracking thread, in the tracking thread, key frames and corresponding depth frames are extracted from the RGB image based on an ORB algorithm, and key frame data are calculated, wherein the key frame data comprise but are not limited to key frame position and posture data; the method comprises the steps that a first map generation module is added with a two-dimensional occupation grid map generation thread, in the two-dimensional occupation grid map generation thread, a dense point cloud map is constructed on the basis of key frames, depth frames corresponding to the key frames and key frame position and posture data, and the dense point cloud map is converted into the two-dimensional occupation grid map; the robot pose calculation module is used for calculating the pose of the robot through the binocular camera parallax principle and calculating the pose of the robot in a two-dimensional occupation grid map through an ORB _ SALM2 algorithm; the second map building module is added with a local map building thread, and in the local map building thread, the key frame information is converted into an internal map based on an ORB _ SALM2 algorithm, wherein the internal map comprises movement data of all key frames, and the movement data comprises but is not limited to a movement track and a movement speed; the optimization module is added with a closed-loop detection thread and a full-set cluster optimization thread, whether key frames are highly matched or not is detected in the closed-loop detection thread, the moving track of the robot forms a closed-loop principle based on the key frame height matching, and the internal map is optimized through the global cluster optimization thread.
The ORB _ SLAM2 algorithm does not provide a map building function for real-time two-dimensional occupancy grid maps, but in this embodiment adds a tracking thread in the tracking module, in which key frames and corresponding depth frames are extracted from RGB images based on the ORB algorithm, and key frame data is calculated, including but not limited to key frame pose data that will be key frames when a new key frame is generated.
In the tracking module, key frames and corresponding depth frames are extracted from the RGB image based on an ORB algorithm, and key frame data are calculated, wherein the specific method comprises the following steps: extracting key points of the RGB image through an ORB algorithm to generate key frames, calculating pose transformation between the two key frames through the change of the relative positions of the key points between the adjacent key frames, predicting speed change between the two key frames, outputting the key frame pose transformation and speed change related data, and generating key frame data.
The essence of the key frame is to reduce the amount of calculation of the ORB _ SLAM2 algorithm, and in the tracking thread, the pose of the key frame is calculated by the position change of the key point between two adjacent frames every time a certain condition is reached (new key point is greater than 20 or the number of frame intervals is greater than 20).
The method comprises the steps that a RGB picture and a depth map corresponding to the RGB picture can be generated, and when the relative positions of two adjacent pictures are known, the two point cloud maps can be combined into a large point cloud map. Based on the above, in the second map construction module, the key frames, the depth frames corresponding to the key frames, and the key frame position and posture data are spliced into the dense point cloud map. And when a new key frame is generated, splicing the key frame, the depth frame corresponding to the key frame and the key frame position and posture data into a dense point cloud map, generating an octree map through the dense point cloud map, and mapping the octree map to 2D to generate a two-dimensional occupation grid map.
The robot pose is calculated through the binocular camera parallax principle, and the method comprises the following steps:
(1) setting two cameras as a left camera and a right camera respectively, wherein the two cameras view the same characteristic point P of a space object at the same moment, and the coordinate of the characteristic point P is (x)c,yc,zc) And the image coordinate of the characteristic point P acquired by the left camera is Pleft＝(Xleft,Yleft) And the image coordinate of the characteristic point P acquired by the right camera is Pright＝(Xright,Yright)；
(2) The images of the two cameras are on the same horizontal plane, and the image coordinates Y coordinates of the characteristic points P are the same, namely Yleft＝YrightY, derived from the trigonometric relationship:
Figure BDA0002661238910000111
Figure BDA0002661238910000112
Figure BDA0002661238910000113
Disparity＝Xleftxright, based on the above-mentioned three-dimensional coordinates of the computed feature points P in the camera coordinate system:
Figure BDA0002661238910000114
Figure BDA0002661238910000115
Figure BDA0002661238910000116
wherein, B represents the distance between the projection center connecting lines of the two cameras;
f denotes a camera focal length.
Any point on the left camera image plane can determine the three-dimensional coordinates of the point as long as the corresponding matching point can be found on the right camera image plane. The method is complete point-to-point operation, and all points on an image surface can participate in the operation as long as corresponding matching points exist, so that corresponding three-dimensional coordinates of the points are obtained.
In the second map building module, the key frame data generated by the tracking thread is converted into an internal map of ORB _ SLAM2 through calculation by the local map building thread, the internal map carries and combines all information sets of data for each key frame, for example, the key frame data only includes the transformation relation between two key frames, the internal map includes the relative changes of all key frames, and generates the data of the key frames, such as the movement track, the movement speed, and the like according to the relative changes.
In the detection module, a closed-loop detection thread is an optimization thread, errors are inevitably generated in a two-dimensional occupation grid map generation thread and a local map construction thread, the errors are larger and larger along with accumulation of running time, the closed-loop detection thread compares key frames to detect whether the key frames are highly matched, and if the key frames are highly matched, closed loop (namely, a robot runs to the position which is originally run) is generated, and the whole map is optimized through the global cluster optimization thread according to the characteristic.
Because the information interaction, execution process, and other contents between the units in the device are based on the same concept as the method embodiment of the present invention, specific contents may refer to the description in the method embodiment of the present invention, and are not described herein again.
In the system, the two-dimensional code is only required to be arranged on the installation pose of the robot, the robot is removed by the camera, and SLAM operation is transferred to the server side, so that the cost and the size of the robot side are reduced, and the computing power of the robot side is released.
Example 3:
an embodiment of the present invention further provides an electronic device, including: at least one memory and at least one processor;
the at least one memory for storing a machine-readable program;
the at least one processor is configured to invoke the machine-readable program to perform the method disclosed in any embodiment 1 of the present invention.
Example 4:
an embodiment of the present invention further provides a computer-readable medium, where a computer instruction is stored on the computer-readable medium, and when the computer instruction is executed by a processor, the processor is caused to execute the method disclosed in any embodiment 1 of the present invention. Specifically, a system or an apparatus equipped with a storage medium on which software program codes that realize the functions of any of the above-described embodiments are stored may be provided, and a computer (or a CPU or MPU) of the system or the apparatus is caused to read out and execute the program codes stored in the storage medium.
In this case, the program code itself read from the storage medium can realize the functions of any of the above-described embodiments, and thus the program code and the storage medium storing the program code constitute a part of the present invention.
Examples of the storage medium for supplying the program code include a floppy disk, a hard disk, a magneto-optical disk, an optical disk (e.g., CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD + RW), a magnetic tape, a nonvolatile memory card, and a ROM. Alternatively, the program code may be downloaded from a server computer via a communications network.
Further, it should be clear that the functions of any one of the above-described embodiments may be implemented not only by executing the program code read out by the computer, but also by causing an operating system or the like operating on the computer to perform a part or all of the actual operations based on instructions of the program code.
Further, it is to be understood that the program code read out from the storage medium is written to a memory provided in an expansion board inserted into the computer or to a memory provided in an expansion unit connected to the computer, and then causes a CPU or the like mounted on the expansion board or the expansion unit to perform part or all of the actual operations based on instructions of the program code, thereby realizing the functions of any of the above-described embodiments.
It should be noted that not all steps and modules in the above flows and system structure diagrams are necessary, and some steps or modules may be omitted according to actual needs. The execution order of the steps is not fixed and can be adjusted as required. The system structure described in the above embodiments may be a physical structure or a logical structure, that is, some modules may be implemented by the same physical entity, or some modules may be implemented by a plurality of physical entities, or some components in a plurality of independent devices may be implemented together.
In the above embodiments, the hardware unit may be implemented mechanically or electrically. For example, a hardware element may comprise permanently dedicated circuitry or logic (such as a dedicated processor, FPGA or ASIC) to perform the corresponding operations. The hardware elements may also comprise programmable logic or circuitry, such as a general purpose processor or other programmable processor, that may be temporarily configured by software to perform the corresponding operations. The specific implementation (mechanical, or dedicated permanent, or temporarily set) may be determined based on cost and time considerations.
While the invention has been shown and described in detail in the drawings and in the preferred embodiments, it is not intended to limit the invention to the disclosed embodiments, and it will be apparent to those skilled in the art that many more embodiments of the invention can be made by combining the various embodiments described above and still fall within the scope of the invention.

Claims (8)

1. A robot positioning method characterized by positioning a robot based on a plurality of cameras fixed externally and a visual SLAM, the method comprising the steps of:
registering the robot and setting a two-dimensional code on the installation pose of the robot, wherein the two-dimensional code contains registration information, is used as a unique identification code of the robot and is used as a pose identification of the robot;
shooting the two-dimensional code through a plurality of cameras fixed outside to obtain an RGB image;
in the tracking thread, extracting key frames and corresponding depth frames from the RGB image based on an ORB algorithm, and calculating key frame data, wherein the key frame data comprises but is not limited to key frame position and posture data;
in a two-dimensional occupation grid map generation thread, constructing a dense point cloud map based on a key frame, a depth frame corresponding to the key frame and key frame position and posture data, and converting the dense point cloud map into a two-dimensional occupation grid map;
calculating the pose of the robot through a binocular camera parallax principle, and calculating the pose of the robot in a two-dimensional occupation grid map through an ORB _ SALM2 algorithm;
in a local map building thread, converting the key frame information into an internal map based on an ORB _ SALM2 algorithm, wherein the internal map comprises movement data of all key frames, and the movement data comprises but is not limited to a movement track and a movement speed;
and in the closed-loop detection thread, detecting whether the key frames are highly matched, and optimizing the internal map through the global cluster optimization thread on the basis of the principle that the moving track of the robot forms a closed loop when the key frames are highly matched.
2. The robot localization method of claim 1, characterized in that converting the dense point cloud map into a two-dimensional occupancy grid map comprises the steps of:
generating an octree map through the dense point cloud map;
mapping the octree map to 2D generates a two-dimensional occupancy grid map.
3. The robot positioning method according to claim 1 or 2, wherein the robot pose is calculated by a binocular camera parallax principle, comprising the steps of:
setting two cameras as a left camera and a right camera respectively, wherein the two cameras view the same characteristic point P of a space object at the same moment, and the coordinate of the characteristic point P is (x)c,yc,zc) And the image coordinate of the characteristic point P acquired by the left camera is Pleft＝(Xleft,Yleft) And the image coordinate of the characteristic point P acquired by the right camera is Pright＝(Xright,Yright)；
The images of the two cameras are on the same horizontal plane, and the image coordinates Y coordinates of the characteristic points P are the same, namely Yleft＝YrightY, derived from the trigonometric relationship:
Figure FDA0002661238900000021
Figure FDA0002661238900000022
Figure FDA0002661238900000023
Disparity＝Xleftxright, based on the above-mentioned three-dimensional coordinates of the computed feature points P in the camera coordinate system:
Figure FDA0002661238900000024
Figure FDA0002661238900000025
Figure FDA0002661238900000026
wherein, B represents the distance between the projection center connecting lines of the two cameras;
f denotes a camera focal length.
4. A robot positioning system, characterized in that it is used for positioning a robot by the robot positioning method according to any one of claims 1-3, a two-dimensional code is set on the installation pose of the robot, the two-dimensional code contains registration information, and is used as a unique identification code of the robot and as a pose identification of the robot; the system comprises:
a registration module to register the robot;
the camera module comprises a camera fixed outside the robot, and the camera is used for shooting the two-dimensional code to obtain an RGB image;
the tracking module is added with a tracking thread, in the tracking thread, key frames and corresponding depth frames are extracted from the RGB images based on an ORB algorithm, and key frame data are calculated, wherein the key frame data comprise but are not limited to key frame position and posture data;
the first map generation module is added with a two-dimensional occupation grid map generation thread, in the two-dimensional occupation grid map generation thread, a dense point cloud map is constructed based on a key frame, a depth frame corresponding to the key frame and key frame position and posture data, and the dense point cloud map is converted into the two-dimensional occupation grid map;
the robot pose calculation module is used for calculating the pose of the robot through the binocular camera parallax principle and calculating the pose of the robot in the two-dimensional occupancy grid map through an ORB _ SALM2 algorithm;
a second map building module, which is added with a local map building thread, and in the local map building thread, the key frame information is converted into an internal map based on an ORB _ SALM2 algorithm, the internal map includes movement data of all key frames, and the movement data includes, but is not limited to, a movement track and a movement speed;
and the optimization module is added with a closed-loop detection thread and a full-set cluster optimization thread, detects whether the key frames are highly matched in the closed-loop detection thread, and optimizes the internal map through the global cluster optimization thread on the basis of the principle that the moving track of the robot forms a closed loop when the key frames are highly matched.
5. The robot positioning system of claim 4, wherein the first map generation module is configured to convert the dense point cloud map into a two-dimensional occupancy grid map by:
generating an octree map through the dense point cloud map;
mapping the octree map to 2D generates a two-dimensional occupancy grid map.
6. The robot positioning system according to claim 1 or 2, characterized in that the robot pose calculation module is configured to calculate the robot pose by:
setting two cameras as a left camera and a right camera respectively, wherein the two cameras view the same characteristic point P of a space object at the same moment, and the coordinate of the characteristic point P is (x)c,yc,zc) And the image coordinate of the characteristic point P acquired by the left camera is Pleft＝(Xleft,Yleft) And the image coordinate of the characteristic point P acquired by the right camera is Pright＝(Xright,Yright)；
The images of the two cameras are on the same horizontal plane, and the image coordinates Y coordinates of the characteristic points P are the same, namely Yleft＝YrightY, derived from the trigonometric relationship:
Figure FDA0002661238900000041
Figure FDA0002661238900000042
Figure FDA0002661238900000043
Disparity＝Xleftxright, based on the above-mentioned three-dimensional coordinates of the computed feature points P in the camera coordinate system:
Figure FDA0002661238900000044
Figure FDA0002661238900000045
Figure FDA0002661238900000046
wherein, B represents the distance between the projection center connecting lines of the two cameras;
f denotes a camera focal length.
7. An electronic device, comprising: at least one memory and at least one processor;
the at least one memory to store a machine readable program;
the at least one processor, configured to invoke the machine readable program, to perform the method of any of claims 1 to 3.
8. A computer readable medium having stored thereon computer instructions which, when executed by a processor, cause the processor to perform the method of any of claims 1 to 3.
CN202010905389.XA 2020-09-01 2020-09-01 Robot positioning method, system, electronic device and computer readable medium Active CN111915680B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010905389.XA CN111915680B (en) 2020-09-01 2020-09-01 Robot positioning method, system, electronic device and computer readable medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010905389.XA CN111915680B (en) 2020-09-01 2020-09-01 Robot positioning method, system, electronic device and computer readable medium

Publications (2)

Publication Number Publication Date
CN111915680A true CN111915680A (en) 2020-11-10
CN111915680B CN111915680B (en) 2024-07-05

Family

ID=73266126

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010905389.XA Active CN111915680B (en) 2020-09-01 2020-09-01 Robot positioning method, system, electronic device and computer readable medium

Country Status (1)

Country Link
CN (1) CN111915680B (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112509061A (en) * 2020-12-14 2021-03-16 济南浪潮高新科技投资发展有限公司 Multi-camera visual positioning method, system, electronic device and medium
CN112991449A (en) * 2021-03-22 2021-06-18 华南理工大学 AGV positioning and mapping method, system, device and medium
CN113325433A (en) * 2021-05-28 2021-08-31 上海高仙自动化科技发展有限公司 Positioning method, positioning device, electronic equipment and storage medium
CN113340293A (en) * 2021-05-28 2021-09-03 上海高仙自动化科技发展有限公司 Positioning method, positioning device, electronic equipment and storage medium

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105869136A (en) * 2015-01-22 2016-08-17 北京雷动云合智能技术有限公司 Collaborative visual SLAM method based on multiple cameras
CN106408609A (en) * 2016-09-13 2017-02-15 江苏大学 Parallel mechanism end motion pose detection method based on binocular vision
US9648303B1 (en) * 2015-12-15 2017-05-09 Disney Enterprises, Inc. Systems and methods for facilitating three-dimensional reconstruction of scenes from videos
CN107657640A (en) * 2017-09-30 2018-02-02 南京大典科技有限公司 Intelligent patrol inspection management method based on ORB SLAM
CN109261528A (en) * 2018-09-03 2019-01-25 广州铁路职业技术学院（广州铁路机械学校） Express delivery sorting method and device based on binocular vision
JP2019074532A (en) * 2017-10-17 2019-05-16 有限会社ネットライズ Method for giving real dimensions to slam data and position measurement using the same
CN109766758A (en) * 2018-12-12 2019-05-17 北京计算机技术及应用研究所 A kind of vision SLAM method based on ORB feature
WO2019169540A1 (en) * 2018-03-06 2019-09-12 斯坦德机器人（深圳）有限公司 Method for tightly-coupling visual slam, terminal and computer readable storage medium
CN110501017A (en) * 2019-08-12 2019-11-26 华南理工大学 A kind of Mobile Robotics Navigation based on ORB_SLAM2 ground drawing generating method
CN111508026A (en) * 2020-04-17 2020-08-07 国网四川省电力公司电力科学研究院 Vision and IMU integrated indoor inspection robot positioning and map construction method

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105869136A (en) * 2015-01-22 2016-08-17 北京雷动云合智能技术有限公司 Collaborative visual SLAM method based on multiple cameras
US9648303B1 (en) * 2015-12-15 2017-05-09 Disney Enterprises, Inc. Systems and methods for facilitating three-dimensional reconstruction of scenes from videos
CN106408609A (en) * 2016-09-13 2017-02-15 江苏大学 Parallel mechanism end motion pose detection method based on binocular vision
CN107657640A (en) * 2017-09-30 2018-02-02 南京大典科技有限公司 Intelligent patrol inspection management method based on ORB SLAM
JP2019074532A (en) * 2017-10-17 2019-05-16 有限会社ネットライズ Method for giving real dimensions to slam data and position measurement using the same
WO2019169540A1 (en) * 2018-03-06 2019-09-12 斯坦德机器人（深圳）有限公司 Method for tightly-coupling visual slam, terminal and computer readable storage medium
CN110462683A (en) * 2018-03-06 2019-11-15 斯坦德机器人（深圳）有限公司 Method, terminal and the computer readable storage medium of close coupling vision SLAM
CN109261528A (en) * 2018-09-03 2019-01-25 广州铁路职业技术学院（广州铁路机械学校） Express delivery sorting method and device based on binocular vision
CN109766758A (en) * 2018-12-12 2019-05-17 北京计算机技术及应用研究所 A kind of vision SLAM method based on ORB feature
CN110501017A (en) * 2019-08-12 2019-11-26 华南理工大学 A kind of Mobile Robotics Navigation based on ORB_SLAM2 ground drawing generating method
CN111508026A (en) * 2020-04-17 2020-08-07 国网四川省电力公司电力科学研究院 Vision and IMU integrated indoor inspection robot positioning and map construction method

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
HONGFAN YANG ET AL,: ""Feature Points Extraction Based on Improved ORB-SLAM"", IEEE, pages 936 - 940 *
魏彤 等: ""动态环境下基于动态区域剔除的双目视觉 SLAM 算法"", 《机器人》, vol. 42, no. 3, pages 336 - 345 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112509061A (en) * 2020-12-14 2021-03-16 济南浪潮高新科技投资发展有限公司 Multi-camera visual positioning method, system, electronic device and medium
CN112509061B (en) * 2020-12-14 2024-03-22 山东浪潮科学研究院有限公司 Multi-camera visual positioning method, system, electronic device and medium
CN112991449A (en) * 2021-03-22 2021-06-18 华南理工大学 AGV positioning and mapping method, system, device and medium
CN112991449B (en) * 2021-03-22 2022-12-16 华南理工大学 AGV positioning and mapping method, system, device and medium
CN113325433A (en) * 2021-05-28 2021-08-31 上海高仙自动化科技发展有限公司 Positioning method, positioning device, electronic equipment and storage medium
CN113340293A (en) * 2021-05-28 2021-09-03 上海高仙自动化科技发展有限公司 Positioning method, positioning device, electronic equipment and storage medium

Also Published As

Publication number Publication date
CN111915680B (en) 2024-07-05

Similar Documents

Publication Publication Date Title
CN112785702B (en) SLAM method based on tight coupling of 2D laser radar and binocular camera
CN111915680A (en) Robot positioning method, system, electronic device and computer readable medium
Asadi et al. Real-time image localization and registration with BIM using perspective alignment for indoor monitoring of construction
CN110568447B (en) Visual positioning method, device and computer readable medium
US10846844B1 (en) Collaborative disparity decomposition
Wendel et al. Natural landmark-based monocular localization for MAVs
KR102198851B1 (en) Method for generating three dimensional model data of an object
JP7422105B2 (en) Obtaining method, device, electronic device, computer-readable storage medium, and computer program for obtaining three-dimensional position of an obstacle for use in roadside computing device
US11600049B2 (en) Perimeter estimation from posed monocular video
Pintore et al. Omnidirectional image capture on mobile devices for fast automatic generation of 2.5 D indoor maps
Coorg et al. Acquisition of a large pose-mosaic dataset
CN108537214B (en) Automatic construction method of indoor semantic map
CN112700486B (en) Method and device for estimating depth of road surface lane line in image
AliAkbarpour et al. Parallax-tolerant aerial image georegistration and efficient camera pose refinement—without piecewise homographies
CN114766042A (en) Target detection method, device, terminal equipment and medium
CN113379748A (en) Point cloud panorama segmentation method and device
WO2022246812A1 (en) Positioning method and apparatus, electronic device, and storage medium
WO2023088127A1 (en) Indoor navigation method, server, apparatus and terminal
Kurz et al. Bundle adjustment for stereoscopic 3d
Grzeszczuk et al. Creating compact architectural models by geo-registering image collections
Chen et al. Epipole Estimation under Pure Camera Translation.
Bartczak et al. Extraction of 3D freeform surfaces as visual landmarks for real-time tracking
Aliakbarpour et al. Imu-aided 3d reconstruction based on multiple virtual planes
Guan et al. Estimation of camera pose with respect to terrestrial LiDAR data
KR102613590B1 (en) Method of determining the location of a drone using 3D terrain location information and a drone thereof

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
TA01 Transfer of patent application right

Effective date of registration: 20240531

Address after: North Zone, 1st Floor, S06 Building, 1036 Inspur Road, High tech Zone, Jinan City, Shandong Province, 250000

Applicant after: Inspur Intelligent IoT Technology Co.,Ltd.

Country or region after: China

Address before: 11-12 / F, building 3, future venture Plaza, north section of Gangxing Third Road, high tech Zone, Jinan City, Shandong Province, 250100

Applicant before: Shandong new generation Information Industry Technology Research Institute Co.,Ltd.

Country or region before: China

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant