A positioning method and system based on visual map

Abstract

This application proposes a positioning method and system based on visual maps, which belongs to the field of UAV mobile robot technology. The method includes: extracting two-dimensional features and descriptors of each frame in the image sequence to be positioned; using the optical flow method to match the two-dimensional features of the current frame image with the two-dimensional features of the previous frame image; in a pre-established static map, collecting all static map frames in which the matched two-dimensional features can be observed, and using the collected static map frames as local static map frames; using a descriptor matching method to match the descriptor of the current frame with the descriptor of the local static map frame, and obtaining a temporary map based on the matching results; under the constraints of the pre-established static map, using sliding window optimization in the temporary map to obtain the optimal estimation results of the current frame, historical frame, and the three-dimensional feature points corresponding to the historical frame to complete positioning. This application improves the mapping accuracy and can complete positioning without the help of external equipment.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) mobile robots, and in particular relates to a positioning method and system based on visual maps. Background Art

[0002] In drone inspections and mobile robot handling operations, ground or flying equipment is usually required to move along specific routes and accurately dock at specific locations to perform tasks. Therefore, repeatable and accurate real-time positioning methods are key to ensuring the automated operation of drones and mobile robots. Common robot autonomous positioning sensors include GPS (Global Positioning System), UWB (Ultra Wide Band), vision and laser sensors: GPS and UWB sensors determine their own position through satellites or external base stations, while vision and laser sensors use SLAM (Simultaneous Localization and Mapping) technology to determine their own position through motion estimation algorithms or based on existing map positioning. Among them, vision sensors are composed of ordinary cameras and are considered to have good application prospects due to their low cost and small size.

[0003] In existing technologies, due to the cumulative errors caused by the use of wheel speed meters, visual / laser odometry, etc., it is difficult to ensure the consistency of repeated positioning of the robot on a fixed path over a long period of time; when the robot moves to a working area where environmental features are missing, map-based positioning will fail; in addition, it is usually necessary to affix QR codes to the robot's working environment for a long time to correct the cumulative errors in the positioning results. Such labels need to be constantly updated and repaired as the equipment is used, adding additional costs to the robot's on-site maintenance environment. Summary of the Invention

[0004] In response to the shortcomings of the existing technology, this application proposes a visual map-based positioning method and system, which does not require GPS or UWB as external positioning basis, and does not require secondary modification of the working environment during the positioning process.

[0005] In the first aspect, this application proposes a positioning method based on a visual map, comprising:

[0006] Step S1: Acquire the image sequence to be located;

[0007] Step S2: extracting two-dimensional features and descriptors of each frame of the image in the image sequence to be located;

[0008] Step S3: using the optical flow method to match the two-dimensional features of the current frame image with the two-dimensional features of the previous frame image to obtain matched two-dimensional features;

[0009] Step S4: In the pre-established static map, all static map frames where the matched two-dimensional features can be observed are collected, and the collected static map frames are used as local static map frames. The pre-established static map includes: static map frames with two-dimensional features and descriptors and static map frames. Figure 3 Dimensional feature points;

[0010] Step S5: using a descriptor matching method to match the descriptor of the current frame image with the descriptor of the local static map frame, and obtaining a temporary map based on the matching results, wherein the temporary map includes: the historical frame image with two-dimensional features and descriptors and the three-dimensional feature points corresponding to the historical frame image;

[0011] Step S6: Under the constraints of the pre-established static map, in the temporary map, sliding window optimization is used to obtain the optimal estimation results of the current frame image, the historical frame image and the three-dimensional feature points corresponding to the historical frame image to complete the positioning.

[0012] The pre-established static map, the establishment process includes:

[0013] Use three-dimensional signs to arrange the mapping space environment;

[0014] Based on the mapping space environment, map data is collected and initialized to obtain an initial static map;

[0015] A global map optimization is performed on the initial static map according to the accumulated error to obtain a pre-established static map.

[0016] The three-dimensional marker is a cube of fixed size, and each side contains a QR code. The content of each QR code is a unique identifier. The three-dimensional marker is only used in the process of establishing a static map. When positioning is performed based on a pre-established static map, all three-dimensional markers are removed.

[0017] Initializing the map data to obtain an initial static map includes:

[0018] Step S102.1: For images in the map data where a 3D marker can be seen, the initial poses of multiple consecutive frames are obtained by using the transformation matrix from the coordinate system corresponding to the 3D marker's QR code to the world coordinate system, and the transformation matrix from the frame coordinate system to the coordinate system corresponding to the 3D marker's QR code. These poses are recorded as the poses of the first type of frames.

[0019] Step S102.2: generating first-category 3D feature points by triangulation based on the pose of the first-category frame;

[0020] Step S102.3: For images in the map data where the 3D marker is not visible, these are designated as second-category frames. Feature matching is performed using descriptors between the images where the 3D marker is not visible and adjacent images where the 3D marker is visible. Correspondences between the 2D feature points of the images where the 3D marker is not visible and the first 3D feature points are obtained. Based on the corresponding first 3D feature points, the pose of the second-category frames is calculated using the PnP method.

[0021] Step S102.4: generating second-type 3D feature points by triangulation based on the poses of the second-type frames and the corresponding image information;

[0022] Step S102.5: Repeat steps S102.1 to S102.2 and steps S102.3 to S102.4 to obtain more poses of the second type of frames and the second type of three-dimensional feature points, and form an initial static map with the poses of all the first type of frames and the corresponding first type of three-dimensional feature points, and the poses of all the second type of frames and the corresponding second type of three-dimensional feature points.

[0023] The performing global map optimization on the initial static map based on the accumulated error to obtain a pre-established static map includes:

[0024] Nonlinear least squares is used to solve the graph optimization model based on stereo marker pose constraints to obtain the optimal estimated values ​​of the frame pose and the optimal estimated values ​​of the three-dimensional feature points. The pre-established static map is composed of the optimal estimated values ​​of all frame poses and the optimal estimated values ​​of the three-dimensional feature points.

[0025] The graph optimization model based on stereoscopic marker pose constraints is calculated as follows:

[0026]

[0027] Among them, M is a graph optimization model based on the three-dimensional marker pose constraint, and the item to be optimized is T wc and P w , and It's T wc and P w The best estimate of The poses of the first type of frames and the poses of the second type of frames are collectively referred to as static map frame poses. Contains all the first-class 3D feature points and the second-class 3D feature points, collectively referred to as static Figure 3 dimensional feature points, f P is the reprojection error, T wci To observe the i-th frame pose of the same QR code of the stereo mark, T wcj is the j-th frame pose of the same QR code observed in the stereo mark, f qis the QR code observation error.

[0028] The two-dimensional code observation error is calculated as follows:

[0029]

[0030] Among them, f q (T wci , T wcj ) is T wci 、T wcj The two-dimensional code observation error between wci To observe the i-th frame pose of the same QR code of the stereo mark, T wcj To observe the j-th frame pose of the same QR code of the stereo mark, the Log() function is an antisymmetric mapping function, R ij is the rotation from the j-th frame pose to the i-th frame pose, t ij is the translation transformation relationship from the j-th frame pose to the i-th frame pose, R wci is the rotation transformation matrix from the i-th frame pose to the world coordinate system, t wci is the translation transformation vector from the i-th frame pose to the world coordinate system, R wcj is the rotation transformation matrix from the j-th frame pose to the world coordinate system, t wcj is the translation transformation vector from the j-th frame pose to the world coordinate system, R ij , t ij The calculation formula is as follows:

[0031]

[0032] Among them, T qci is the transformation matrix from the i-th frame pose to the observed QR code coordinate system, T qcj is the transformation matrix from the j-th frame pose to the observed QR code coordinate system.

[0033] The visual map-based positioning method further includes:

[0034] When the current frame image is the initial frame and the previous frame image cannot be obtained, the initial positioning method is started;

[0035] The initial positioning method includes:

[0036] Step S0.1: Given an initial pose;

[0037] Step S0.2: Search in the pre-established static map to obtain the map frame closest to the initial pose;

[0038] Step S0.3: Use the descriptor matching method to match the current frame image with the nearest static map frame, and establish the two-dimensional feature points on the current frame image and the static map frame. Figure 3The correspondence between dimensional feature points;

[0039] Step S0.4: Based on the two-dimensional feature points on the current frame image and the static ground Figure 3 The PnP method is used to obtain the initial positioning pose based on the correspondence between the feature points.

[0040] The visual map-based positioning method further includes:

[0041] When the newly acquired image to be positioned exceeds the pre-established static map, temporary three-dimensional feature points are obtained by using the historical frame image and the local static map frame, and the correspondence between the two-dimensional feature points on the current frame image and the temporary three-dimensional feature points is established. Then, the PnP method is used to obtain the positioning pose of the newly acquired image to be positioned;

[0042] The temporary three-dimensional feature points are obtained as follows:

[0043] Use the frame queue to save the third type of frames from the current time t to the historical time Δt, and record the third type of frames as temporary frames;

[0044] By using the 2D feature matching relationship between the temporary frame and the local static map frame, and the 2D feature matching relationship between each temporary frame, the third type of feature points are obtained through triangulation method and recorded as temporary 3D feature points.

[0045] The visual map-based positioning method further includes:

[0046] Calculate the ratio of the number of 3D feature points that are successfully paired with the descriptor of the current frame image and the descriptor of the local static map frame to the number of corresponding 2D features that are successfully paired;

[0047] When the ratio is less than the set threshold, the map is considered lost, that is, the image sequence to be located deviates from the pre-established static map. After step S4 is completed, a thread is added, and the previous initial positioning posture is used as the given initial posture, and steps S0.2 to S0.3 are executed, and then step S5 is turned to.

[0048] In a second aspect, the present application proposes a positioning method system based on a visual map, comprising:

[0049] A data acquisition module, used for acquiring an image sequence to be located;

[0050] A feature extraction module, configured to extract two-dimensional features and descriptors of each frame of the image in the image sequence to be positioned;

[0051] A feature matching module is used to match the two-dimensional features of the current frame image with the two-dimensional features of the previous frame image using an optical flow method to obtain matched two-dimensional features;

[0052] a feature collection module, configured to collect, in a pre-established static map, all static map frames in which the matched two-dimensional features can be observed, and use the collected static map frames as local static map frames, wherein the pre-established static map includes: static map frames having two-dimensional features and descriptors, and three-dimensional feature points corresponding to the static map frames;

[0053] A temporary map construction module is configured to match the descriptor of the current frame image with the descriptor of the local static map frame using a descriptor matching method, and obtain a temporary map based on the matching results. The temporary map includes: a historical frame image with two-dimensional features and descriptors, and three-dimensional feature points corresponding to the historical frame image;

[0054] The positioning result acquisition module is used to obtain the optimal estimation results of the current frame image, historical frame image and the three-dimensional feature points corresponding to the historical frame image in the temporary map under the constraints of the pre-established static map, and complete the positioning.

[0055] Beneficial effects:

[0056] This application proposes a positioning method and system based on visual maps. While ensuring positioning accuracy, the equipment is simple and easy to install. Compared with the positioning solution of installing QR codes and UWB sensors, this application does not require the use of GPS or UWB as an external positioning basis. During the production and operation of the robot, this application always locates the 3D feature points based on the visual map, without requiring any modification to the working environment, which is conducive to subsequent equipment maintenance work.

[0057] The positioning results of this application are more robust. Even if the camera deviates from the pre-built map environment, it can still achieve reliable real-time camera pose estimation with the help of temporary map point information. At the same time, when the camera observes the pre-built map environment, the static map points matched by image retrieval are used to correct the cumulative positioning error caused by sliding window optimization, thereby ensuring that the visual positioning results still have accurate and repeatable positioning accuracy when the robot runs back and forth on the working path. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 Flowchart of the visual map positioning method according to an embodiment of the present application;

[0059] Figure 2 A flowchart of a visual map positioning method according to an embodiment of the present application;

[0060] Figure 3 Schematic diagram of a three-dimensional logo according to an embodiment of the present application;

[0061] Figure 4 Schematic diagram of the layout and mapping space of the embodiment of the present application;

[0062] Figure 5 A schematic diagram of whether the motion trajectory of a stereoscopic marking camera can be observed in an embodiment of the present application;

[0063] Figure 6 Schematic diagram of the initialization process of an embodiment of the present application;

[0064] Figure 7 Graph optimization model combining stereoscopic marker pose constraints in the embodiment of the present application

[0065] Figure 8 Schematic diagram of feature matching inspired by optical flow in an embodiment of the present application;

[0066] Figure 9 Schematic diagram of the relationship between the local static map frame and the current frame image in the embodiment of the present application;

[0067] Figure 10 Schematic diagram of the sliding window optimization model of the embodiment of the present application;

[0068] Figure 11 Schematic diagram of two queue containers in an embodiment of the present application;

[0069] Figure 12 Principle block diagram of the visual map-based positioning system in an embodiment of the present application. DETAILED DESCRIPTION

[0070] The specific implementation of the present application is further described in detail below with reference to the accompanying drawings and examples.

[0071] In the existing technology, the papers “Tong, Qin, Peiliang, et al. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator [J]. IEEE Transactions on Robotics, 2018. DOI: 10.1109 / TRO.2018.2853729.” and “Mur-Artal R, Montiel JMM, Tardos J D. ORB-SLAM: A Versatile and Accurate Monocular SLAM System [J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163. DOI: 10.1109 / TRO.2015.2463671.” use the visual odometry method for positioning and add loop detection to correct the accumulated error of the odometer to improve positioning accuracy; the paper “Cao S, Lu X, Shen S. GVINS: Tightly Coupled GNSS–Visual–Inertial Fusion for Smooth and Consistent State Estimation[J].IEEE Transactions on Robotics:A publication of the IEEE Robotics and Automation Society,2022(4):38." GPS-assisted visual IMU odometer is used to reduce the accumulation of self-motion estimation errors, thereby ensuring the long-term consistent movement of the robot on a fixed path. The invention patent "201911006031.7 A UAV positioning and navigation method for indoor coal yards" adds a UWB base station on the basis of GPS, and selects GPS or UWB as the global positioning result to be fused with the visual SLAM result according to the confidence parameters of the GPS and UWB positioning results.

[0072] Furthermore, autonomous robot positioning can also be achieved by utilizing environmental information or modifying the environment. The invention patent "201811308853.6 Mobile Robot Offline Map Preservation and Real-time Relocalization Method" constructs an ORB map and saves it as a binary file, enabling offline visualization of the visual map and positioning based on the saved map, ensuring the repeatable positioning accuracy of the mobile robot. The invention patent "201711075798.6 Visual Positioning Method Based on ORB Sparse Point Cloud and QR Code" installs a fixed QR code marker in the working environment. When the camera detects the QR code, QR code-based positioning is enabled to correct the accumulated errors generated during the visual odometry positioning process.

[0073] However, positioning methods based on self-motion estimation, such as those using wheel speed meters, visual / laser odometry, etc., have cumulative errors and are difficult to ensure the consistency of repeated positioning of the robot on a long-term fixed path. The loop detection function in the system can only compensate for the consistency error of repeated positioning in some small scenes. In addition, relying on GPS sensors, it is difficult to ensure the accuracy of repeated positioning in scenarios where GPS fails. The invention patent "201811308853.6 Mobile Robot Offline Map Storage and Real-time Repositioning Method" uses pre-built environmental information for positioning, but when the robot moves to a working area where environmental features are missing, the map-based positioning will fail. The invention patent "201711075798.6 Visual Positioning Method Based on ORB Sparse Point Cloud and QR Code" requires the long-term affixation of QR codes to the robot's working environment to correct the cumulative errors in the positioning results. Such labels need to be constantly updated and repaired as the equipment is used, adding additional costs to the robot's maintenance in the on-site environment.

[0074] This application proposes a positioning method and system based on visual maps to achieve autonomous visual positioning of UAVs or mobile robots in fixed environments. The main idea is to build a visual map of the robot's working environment, and then use the visual map and visual odometry to perform autonomous positioning of the robot. The advantage is that there is no need to rely on GPS or UWB as an external positioning basis, and there is no need to perform secondary transformation of the working environment (such as pasting a QR code) during the positioning process. Compared with the traditional visual map-based solution, this application has two improvements: 1. In terms of visual map construction, since the map accuracy directly affects the robot positioning accuracy, this application designs a user-participated global map optimization strategy to improve the mapping accuracy; 2. In the map-based positioning process, temporary features are created and combined with the existing map for positioning to ensure that the robot can still be positioned through motion estimation when it moves to an unmapped area, and the accumulated positioning error caused by motion estimation can be corrected after the visual map is detected.

[0075] Example 1:

[0076] This application proposes a positioning method based on visual maps, such as Figure 1 、 Figure 2 Shown, including:

[0077] Step S1: Acquire the image sequence to be located;

[0078] In this embodiment, since it is a continuous positioning process, the camera continuously collects images to form a video stream, but each calculation cycle is based on one image. Therefore, this embodiment uses a camera to obtain the image sequence to be positioned. In actual applications, other methods can also be used to obtain the image sequence to be positioned.

[0079] Step S2: extracting two-dimensional features and descriptors of each frame of the image in the image sequence to be located;

[0080] It is understandable that conventional techniques can be used to extract the two-dimensional features and descriptors of each frame of image.

[0081] Step S3: using the optical flow method to match the two-dimensional features of the current frame image with the two-dimensional features of the previous frame image to obtain matched two-dimensional features;

[0082] In this embodiment, the optical flow method uses the temporal changes in pixels in an image sequence and the correlation between adjacent frames to find the correspondence between the previous and current frames, thereby calculating the motion information of objects between adjacent frames. Optical flow is the instantaneous speed of pixels moving on the observation imaging plane of a moving object in space. It not only contains the motion information of the observed object, but also contains the three-dimensional structural information of the scene.

[0083] This embodiment uses the optical flow method to match the features of the current frame image with the previous frame image, which is based on continuous image acquisition. Since the image changes continuously, the optical flow method can accurately estimate the changes in the feature points on the image. By tracking the optical flow of the two-dimensional feature points on the previous frame image, the range interval where the corresponding two-dimensional feature points on the current frame image should be located is obtained. If there is only one two-dimensional feature in the interval, the current feature is paired with the feature on the previous frame image. If there are multiple features in the interval, the two-dimensional feature with the highest descriptor similarity is paired with the two-dimensional feature on the previous frame image based on the similarity of the descriptors.

[0084] Step S4: In the pre-established static map, all static map frames where the matched two-dimensional features can be observed are collected, and the collected static map frames are used as local static map frames. The pre-established static map includes: static map frames with two-dimensional features and descriptors and three-dimensional feature points corresponding to the static map frames; wherein the three-dimensional feature points corresponding to the static map frames are the static map frames. Figure 3 Dimensional feature points.

[0085] In this embodiment, a static map needs to be pre-established. The constructed static map is consistent with the size of the real environment and effectively reduces the error accumulation and scale drift problems of traditional visual SLAM algorithms.

[0086] The pre-established static map, the establishment process includes:

[0087] Step S101: using three-dimensional markers to arrange the mapping space environment;

[0088] In this embodiment, when creating a static map, it is necessary to first install a three-dimensional marker. The purpose of installing a three-dimensional marker is to improve the map accuracy and can be removed after the map is built, reducing the maintenance cost of the positioning system during the long-term operation of the equipment. Figure 3 As shown in the figure, it is mainly composed of a cube with multiple ArUco (or AprilTag or other QR code graphics) attached. The cube size is fixed and the QR code content on each side is a unique ID. Using the above three-dimensional logo, the environment of the mapping space is arranged, as shown in the figure. Figure 4 The position of the 3D marker in the world coordinate system is obtained using conventional measurement methods (such as total stations and 3D scanners). The forward, left, and upward directions of the 3D marker coincide with the northeast celestial coordinate system. A level and magnetometer are used to measure and correct the 3D marker's installation posture error. Given the known size and installation posture of the 3D marker, the position of each QR code in the world coordinate system can be calculated.

[0089] Step S102: Based on the mapping space environment, map data is collected and initialized to obtain an initial static map;

[0090] In this embodiment, after the three-dimensional markers are installed, it is necessary to collect map data and initialize it. The map data collection content is the image data captured by the camera during continuous motion. Data initialization refers to the use of the collected information in combination with the three-dimensional markers to obtain the initial map data. After the environment layout is completed, the robot can be operated or the camera can be used directly to collect the environment image. Due to the influence of the on-site environment in actual use, the number of three-dimensional markers set is limited; and due to the limitation of the camera's field of view, the three-dimensional markers cannot always be observed in the collected environment pictures. Figure 5 As shown, on the camera motion trajectory, the black lines are where the marker blocks can be seen, and the images captured at the gray lines cannot observe any marker blocks. In this case, the initialization method for seeing the stereo markers is different from that for not seeing the stereo markers. Initializing the map data to obtain the initial static map includes:

[0091] Step S102.1: For images in the map data where a 3D marker can be seen, the initial poses of multiple consecutive frames are obtained by using the transformation matrix from the coordinate system corresponding to the 3D marker's QR code to the world coordinate system, and the transformation matrix from the frame coordinate system to the coordinate system corresponding to the 3D marker's QR code. These poses are recorded as the poses of the first type of frames.

[0092] In this embodiment, the initialization process is as follows Figure 6 As shown, process ① is a method for obtaining frame pose based on stereo identification. wq T is the transformation matrix from the coordinate system corresponding to the QR code of the three-dimensional mark to the world coordinate system, which is obtained through pre-surveying; qc is the transformation matrix from the frame coordinate system to the coordinate system corresponding to the QR code of the stereo mark, which is obtained by calculation using the QR code; wc is the transformation matrix from the frame coordinate system to the world coordinate system, that is, the first frame pose, and the calculation formula is as follows:

[0093] T wc =T wq ·T qc (1)

[0094] Step S102.2: According to the pose of the first frame, generate the first type of three-dimensional feature points by triangulation method. The first type of three-dimensional feature points are static Figure 3 A part of the dimension feature point;

[0095] In this embodiment, Figure 6 Process ② is the process of generating the three-dimensional coordinates of feature points from images with known poses. The two-dimensional feature points on the image are obtained by the superPoint algorithm, and the matching of two-dimensional feature points between images is completed by superPoint descriptor matching. After completing the two-dimensional feature point matching on multiple images, the three-dimensional coordinates P of the feature points on the frame with known poses are calculated by triangulation method. w , that is, the first three-dimensional feature point.

[0096] Step S102.3: For images in the map data where the 3D marker is not visible, these are designated as second-category frames. Feature matching is performed using descriptors between the images where the 3D marker is not visible and adjacent images where the 3D marker is visible. Correspondences between the 2D feature points of the images where the 3D marker is not visible and the first 3D feature points are obtained. Based on the corresponding first 3D feature points, the pose of the second-category frames is calculated using the PnP method.

[0097] In this embodiment, Figure 6Process ③ is the pose calculation process for frames where the QR code cannot be observed. For frames where the QR code of the stereo logo cannot be observed, the two-dimensional feature points on the image and the three-dimensional coordinates P generated by process ② can be obtained through feature matching based on the superPoint descriptor. w Correspondence, so as to use Perspective-n-Point (PnP) to calculate the pose T of the frame wc , that is, the second frame pose, T′ wc With T wc Generated by different observation information, but express the same meaning.

[0098] Step S102.4: According to the position and corresponding image information of the second type of frame, generate the second type of three-dimensional feature points by triangulation method. The second type of three-dimensional feature points are also static. Figure 3 A part of the dimension feature point;

[0099] Step S102.5: Repeat steps S102.1 to S102.2 and steps S102.3 to S102.4 to obtain more poses of the second type of frames and the second type of three-dimensional feature points, and form an initial static map with the poses of all the first type of frames and the corresponding first type of three-dimensional feature points, and the poses of all the second type of frames and the corresponding second type of three-dimensional feature points.

[0100] In this embodiment, Figure 6 In process ④, use the method in process ② to generate new feature point 3D coordinates P′ based on the image with known pose w , and process ③ is repeated alternately to obtain the poses of all frames in the map and the three-dimensional coordinates of the feature points on the frames.

[0101] Step S103: performing global map optimization on the initial static map according to the accumulated error to obtain a pre-established static map;

[0102] In this embodiment, due to the influence of observation noise, the initial static map construction method is prone to cumulative errors during the iteration process. This embodiment designs a graph optimization model that combines the pose constraints of three-dimensional markers, such as Figure 7 shown.

[0103] The performing global map optimization on the initial static map based on the accumulated error to obtain a pre-established static map includes:

[0104] Nonlinear least squares is used to solve the graph optimization model based on stereo marker pose constraints to obtain the optimal estimated values ​​of the frame pose and the optimal estimated values ​​of the three-dimensional feature points. The pre-established static map is composed of the optimal estimated values ​​of all frame poses and the optimal estimated values ​​of the three-dimensional feature points.

[0105] The image frame pose and 3D map point coordinates in the map are the parameters to be optimized, which include two types of observation errors: reprojection error and QR code observation error. The graph optimization model based on the 3D marker pose constraint is calculated as follows:

[0106]

[0107] Among them, M is a graph optimization model based on the three-dimensional marker pose constraint, and the item to be optimized is T wc and P w , and It's T wc and P w The best estimate of The poses of the first type of frames and the poses of the second type of frames are collectively referred to as static map frame poses. Contains all the first-class 3D feature points and the second-class 3D feature points, collectively referred to as static Figure 3 dimensional feature points. wci To observe the i-th frame pose of the same QR code of the stereo mark, T wcj is the j-th frame pose of the same QR code observed in the stereo mark, f q is the observation error of the QR code, f P Represents the reprojection error, which is expressed in the same form as the general visual SLAM error. The calculation formula is as follows:

[0108]

[0109] Among them, (u, v) T is the coordinate of the feature point on the image, f x is the calculated focal length of the camera in the x-axis direction, f y is the calculated focal length of the camera in the y-axis direction, c x is the calculated principal point in the x-axis direction of the camera, c x is the calculated principal point in the y-axis direction of the camera, R wc is the rotation transformation matrix from the frame coordinate system to the world coordinate system, t wc is the rotation and translation transformation vector from the frame coordinate system to the world coordinate system, represented by T wc get.

[0110] f q It represents the observation error of the QR code between two frames of images, and is calculated as follows:

[0111]

[0112] Among them, f q (T wci , T wcj ) is T wci 、T wcjThe two-dimensional code observation error between wci To observe the i-th frame pose of the same QR code of the stereo mark, T wcj To observe the j-th frame pose of the same QR code of the stereo mark, the Log() function is an antisymmetric mapping function, R ij is the rotation from the j-th frame pose to the i-th frame pose, t ij is the translation transformation relationship from the j-th frame pose to the i-th frame pose. Since the i and j-frame images observe the same QR code logo at the same time, their relative pose relationship can be determined by the QR code logo. wci is the rotation transformation matrix from the i-th frame pose to the world coordinate system, t wci is the translation transformation vector from the i-th frame pose to the world coordinate system, R wcj is the rotation transformation matrix from the j-th frame pose to the world coordinate system, t wcj is the translation transformation vector from the j-th frame pose to the world coordinate system, T wci and T wcj Indicates the two-frame poses of the same QR code observed, (R wci , t wci ) and (R w c j , t wcj ) respectively from T wci and T wcj , R ij and t ij It is obtained by estimating the pose of the QR code, as shown in formula (5). qcj With T qcj They represent the transformation matrices from the two image frames to the QR code coordinate system, which are obtained by Perspective-n-Point calculation.

[0113]

[0114] Among them, T qci is the transformation matrix from the i-th frame pose to the observed QR code coordinate system, which is obtained by calculating the QR code identification. qcj is the transformation matrix from the j-th frame pose to the observed QR code coordinate system, which is also obtained by QR code identification calculation.

[0115] In this embodiment, compared to directly locating the frame pose using the QR code, this embodiment establishes a constraint equation by observing the relative pose relationship of the same QR code in different frames. This is more adaptable to different field environments and reduces the frame pose estimation error introduced by the installation error limit of the stereo sign.

[0116] Finally, the optimal estimate of formula (2) is calculated using nonlinear least squares methods such as gradient descent method or Newton method. and That is, the frame pose and three-dimensional coordinates of the feature points in the map.

[0117] Step S5: using a descriptor matching method to match the descriptor of the current frame image with the descriptor of the local static map frame, and obtaining a temporary map based on the matching results, wherein the temporary map includes: the historical frame image with two-dimensional features and descriptors and the three-dimensional feature points corresponding to the historical frame image;

[0118] Step S6: Under the constraints of the pre-established static map, in the temporary map, sliding window optimization is used to obtain the optimal estimation results of the current frame image, the historical frame image and the three-dimensional feature points corresponding to the historical frame image to complete the positioning.

[0119] The visual map-based positioning method further includes:

[0120] When the current frame image is the initial frame and the previous frame image cannot be obtained, the initial positioning method is started;

[0121] The initial positioning method includes:

[0122] Step S0.1: Given an initial pose;

[0123] Step S0.2: Search in the pre-established static map to obtain the map frame closest to the initial pose;

[0124] Step S0.3: Use the descriptor matching method to match the current frame image with the nearest static map frame, thereby establishing the two-dimensional feature points on the current frame image and the static map frame. Figure 3 The correspondence between dimensional feature points;

[0125] Step S0.4: Based on the two-dimensional feature points on the current frame image and the static ground Figure 3 The PnP method is used to obtain the initial positioning pose based on the correspondence between the feature points.

[0126] The visual map-based positioning method further includes:

[0127] When the newly acquired image to be located exceeds the pre-established static map, the historical frame image cannot establish the relationship between the two-dimensional feature points on the current frame image and the static map. Figure 3 In order to ensure uninterrupted positioning, temporary three-dimensional feature points are obtained by using historical frame images and map frames, and the correspondence between the two-dimensional feature points and the temporary three-dimensional feature points on the current frame image is established, and then the PnP method can be used to obtain the positioning posture of the newly acquired image.

[0128] The method for obtaining temporary three-dimensional feature points is as follows:

[0129] A frame queue is used to save the third type of frames from the current time t to the historical time Δt. Since it is a continuous positioning process, the pose of the third type of frames has been estimated before, and the third type of frames are recorded as temporary frames.

[0130] By using the triangulation method, the 2D feature matching relationship between the temporary frame and the map frame, and the 2D feature matching relationship between the temporary frames, the third type of feature points are triangulated and recorded as temporary 3D feature points. The method for determining map loss is as follows:

[0131] Calculate the ratio of the number of 3D feature points that are successfully paired with the descriptor of the current frame image and the descriptor of the local static map frame to the number of corresponding 2D features that are successfully paired;

[0132] When the ratio is less than the set threshold, the map is considered lost, that is, the image sequence to be positioned deviates from the pre-established static map. At this time, the robot can still be positioned, but due to the lack of a static map reference, as the robot moves, the positioning result will have cumulative errors. In order to eliminate the cumulative errors, a thread is added after step S4 is completed, and the previous initial positioning posture is used as the given initial posture, and steps S0.2 to S0.3 are executed, and then go to step S5.

[0133] In order to describe the positioning method based on the visual map in more detail (excluding the detailed process of the pre-established static map), an example is given and described as follows:

[0134] Since the environment map is constructed using 3D point features, in the map-based positioning process, this example no longer requires identification blocks to assist in positioning, thereby achieving the effect of real-time positioning without modifying the on-site environment. Since the pre-constructed map is scaled, the present invention only uses a single camera as a sensor to achieve scaled robot pose estimation in the working environment. Furthermore, when the robot walks out of the map environment or the on-site environment changes, the positioning algorithm of the present invention can still maintain the robot's continuous positioning needs through temporary map data. The example process of the visual map positioning method is as follows: Figure 2 As shown:

[0135] Among them, the method for retrieving the static map in step ① is to compare the difference between the given pose and the pose of the frame in the static map. Among all static map frames whose orientation difference from the current initial pose is less than 90°, the frame with the smallest distance to the initial position is identified as the closest map frame.

[0136] Step 2 also uses the superPoint descriptor matching method to match the current frame image with the nearest map frame. After pairing the 2D feature points between the two frames, the three-dimensional coordinates corresponding to the 2D feature points on the map frame can be used to determine the three-dimensional coordinates corresponding to the 2D feature points on the current frame image.

[0137] Step 3 uses matched feature points. After matching the 2D point pairs between the current frame image and the nearest map frame, the 3D coordinates of the 2D feature points in the nearest map frame are obtained based on their coordinates on the map. In practice, not all feature points in an image can be successfully matched, so pose estimation only uses successfully matched feature points. The frame pose estimation method is the Perspective-n-Point method.

[0138] Step ④ matches the features of the current frame image with the previous frame image through optical flow inspiration, which is based on the continuous acquisition of images. Since the image changes continuously, the optical flow method can accurately estimate the changes of the feature points on the image. Figure 8 As shown in the figure. By tracking the optical flow of the feature points on the previous frame image, the range interval of the corresponding feature points on the current frame image is obtained. If there is only one feature in the interval, the current feature is paired with the feature on the previous frame image. If there are multiple features in the interval, the feature with the highest descriptor similarity is paired with the feature on the previous frame image based on the similarity of the descriptors.

[0139] Step ⑤ is based on the principle of collecting local static frames based on the matched feature points. It uses the feature points matched in step ④ to collect all static map frames that can observe this feature point. The relationship between the local static map frame and the current frame image is as follows: Figure 9 shown.

[0140] Step 6: The current frame image is matched with the local static map frame using the same superPoint descriptor matching method. By matching the current frame image with multiple frames in the local map, the 2D features in the current frame image can be paired with 3D feature points in more maps.

[0141] Step 7: Pose estimation is implemented using a sliding window optimization strategy. The sliding window refers to the image information set observed over a period of time and the temporary triangulated 3D feature point set. The sliding window optimization model is as follows: Figure 10 As shown in the figure, after sliding window optimization, the current frame image, the historical frame images in the temporary map, and the three-dimensional coordinates of the feature points all obtain the optimal estimation results, completing the positioning.

[0142] Figure 10 In the static map frame and static Figure 3The three-dimensional coordinates of the feature points are fixed state information, obtained when the environment map is constructed, and do not change with iterative optimization; the temporary map history frame image (including the current frame image) is the image information collected by the camera in a short period of time, and the three-dimensional coordinates of the feature points of the temporary map are the three-dimensional feature points obtained by triangulating between the temporary map history frame images and between the temporary map frame and the static map frame. The temporary map information is the state information to be optimized; the dotted line in the figure is the observation error, which is the same as the reprojection error described in formula (3). Unlike the sliding window described in the traditional visual odometry (such as VINS, Velocity Inertial Navigation System, which means speed inertial navigation system in Chinese), this example uses a pre-built static map to constrain the frame pose estimation result in the sliding window, ensuring that the scale of the temporary map is consistent with the static map without the help of IMU. Figure 1 At the same time, the observation error between the static map frame and the three-dimensional coordinates of the temporary map feature points, and the observation error between the static map points and the temporary map frame are generated, which effectively suppresses the cumulative error of the temporary map data caused by the long-term sliding window optimization and ensures the long-term repeated positioning accuracy.

[0143] Step ⑧ Triangulate temporary map points. This is designed for when the environment changes temporarily or the robot enters an area outside the map, to avoid the current frame image from intersecting with the static map. Figure 3 The positioning failure caused by the failure of 3D feature matching is solved. The method is that after the current frame image is successfully matched with the historical frame image and the static frame, if the 2D feature points that are successfully matched on the historical frame image and the static frame have not been triangulated (that is, there is no corresponding 3D feature point on the historical frame image), the 3D coordinates of this feature point are generated by triangulation. In the positioning process, the newly generated 3D feature points are temporary. Figure 3 Dimensional feature points.

[0144] Step ⑨ The method of caching the current frame image and temporary map points is to create two queue containers to store temporary frames and temporary map points respectively, such as Figure 11 As shown in the figure, the frame queue stores images from the current moment to the historical moment Δt. Each time step 9 is executed, it is determined whether the image acquisition time at the end of the frame queue lags behind t-Δt. If so, the image data at the end of the queue is ejected. The 3D point queue stores all temporary map points that can be observed from the current moment to t-Δt. Each time step 9 is executed, it is determined whether the 3D point at the end of the queue has been observed in the frames at or after t-Δt. If not, the 3D point data at the end of the queue is ejected.

[0145] Step ⑩ is implemented based on the number of static map points that can be observed in the current frame image, and the calculation formula is as follows:

[0146]

[0147] Among them, n s is the number of 3D feature points that are successfully matched between the current frame image and the static map frame, n c The total number of 2D feature points extracted for the current frame image. If the k value is less than 0.2, the current map is considered lost. This means that if there are fewer observed static map points, it indicates that the current robot's observed pose has deviated from the map environment, or the map environment has changed. If the map is lost, after the next step 5 is completed, an additional thread will be added to execute steps 1 and 2. In this case, step 1 uses the initial pose as the last positioning pose.

[0148] This embodiment proposes a visual map-based positioning method, which uses temporary map data to save historical frame images and temporarily created 3D feature points, and designs a sliding window optimization method to optimize the historical frame images and temporary feature points. The positioning result of this embodiment has stronger robustness. Even if the camera deviates from the pre-constructed map environment, we can still use the temporary map point information to achieve reliable real-time camera pose estimation; at the same time, when the camera observes the pre-constructed map environment, the static map points matched by image retrieval are used to correct the cumulative positioning error caused by the sliding window optimization, thereby ensuring that the visual positioning result still has accurate and repeatable positioning accuracy when the robot runs back and forth on the working path.

[0149] Compared with the multi-sensor fusion positioning solution, the equipment of this embodiment is simple and easy to install while ensuring positioning accuracy; compared with the positioning solution of installing QR codes and UWB sensors, during the robot's production operation, the present invention always uses 3D feature point positioning based on the visual map, without requiring any modification to the working environment, which is conducive to later equipment maintenance work.

[0150] This example also proposes how to construct a static map based on stereo marker blocks. This includes a frame pose optimization algorithm for observing multiple ArUco markers from a single image, and a global map construction algorithm that combines fixed and optimized frames. The map constructed in this example is consistent with the scale of the real environment and effectively reduces the error accumulation and scale drift problems of traditional visual SLAM algorithms.

[0151] Example 2:

[0152] This embodiment proposes a positioning system based on a visual map, such as Figure 12 The module includes: a data acquisition module, a feature extraction module, a feature matching module, a feature collection module, a temporary map construction module, and a positioning result acquisition module, wherein the data acquisition module, the feature extraction module, the feature matching module, the feature collection module, the temporary map construction module, and the positioning result acquisition module are connected in sequence;

[0153] A data acquisition module, used for acquiring an image sequence to be located;

[0154] A feature extraction module, configured to extract two-dimensional features and descriptors of each frame of the image in the image sequence to be positioned;

[0155] A feature matching module is used to match the two-dimensional features of the current frame image with the two-dimensional features of the previous frame image using an optical flow method to obtain matched two-dimensional features;

[0156] a feature collection module, configured to collect, in a pre-established static map, all static map frames in which the matched two-dimensional features can be observed, and use the collected static map frames as local static map frames, wherein the pre-established static map includes: static map frames having two-dimensional features and descriptors, and three-dimensional feature points corresponding to the static map frames;

[0157] A temporary map construction module is configured to match the descriptor of the current frame image with the descriptor of the local static map frame using a descriptor matching method, and obtain a temporary map based on the matching results. The temporary map includes: a historical frame image with two-dimensional features and descriptors, and three-dimensional feature points corresponding to the historical frame image;

[0158] The positioning result acquisition module is used to obtain the optimal estimation results of the current frame image, historical frame image and the three-dimensional feature points corresponding to the historical frame image in the temporary map under the constraints of the pre-established static map, and complete the positioning.

[0159] The various embodiments in this application are described in a progressive manner, and the same or similar parts between the various embodiments can be referred to each other. Each embodiment focuses on the differences from other embodiments.

[0160] The scope of protection of this application is not limited to the above-described embodiments. Obviously, those skilled in the art may make various modifications and variations to this disclosure without departing from the scope and spirit of this disclosure. If such modifications and variations fall within the scope of the claims of this disclosure and their equivalents, the disclosure is intended to include such modifications and variations.

Claims

1. A positioning method based on visual maps, characterized in that: include: Step S1: Acquire the image sequence to be located; Step S2: extracting two-dimensional features and descriptors of each frame of the image in the image sequence to be located; Step S3: using the optical flow method to match the two-dimensional features of the current frame image with the two-dimensional features of the previous frame image to obtain matched two-dimensional features; Step S4: In a pre-established static map, all static map frames in which the matched two-dimensional features can be observed are collected, and the collected static map frames are used as local static map frames, wherein the pre-established static map includes: static map frames with two-dimensional features and descriptors, and static map three-dimensional feature points; Step S5: using a descriptor matching method to match the descriptor of the current frame image with the descriptor of the local static map frame, and obtaining a temporary map based on the matching results, wherein the temporary map includes: the historical frame image with two-dimensional features and descriptors and the three-dimensional feature points corresponding to the historical frame image; Step S6: Under the constraints of the pre-established static map, in the temporary map, sliding window optimization is used to obtain the optimal estimation results of the current frame image, the historical frame image and the three-dimensional feature points corresponding to the historical frame image to complete the positioning.

2. The method for positioning based on a visual map according to claim 1, characterized in that: The pre-established static map, the establishment process includes: Use three-dimensional signs to arrange the mapping space environment; Based on the mapping space environment, map data is collected and initialized to obtain an initial static map; A global map optimization is performed on the initial static map according to the accumulated error to obtain a pre-established static map.

3. The method for positioning based on a visual map according to claim 2, characterized in that: The three-dimensional marker is a cube of fixed size, and each side contains a QR code. The content of each QR code is a unique identifier. The three-dimensional marker is only used in the process of establishing a static map. When positioning is performed based on a pre-established static map, all three-dimensional markers are removed.

4. The method for positioning based on a visual map according to claim 2, characterized in that: Initializing the map data to obtain an initial static map includes: Step S102.1: For images in the map data where a 3D marker can be seen, the initial poses of multiple consecutive frames are obtained by using the transformation matrix from the coordinate system corresponding to the 3D marker's QR code to the world coordinate system, and the transformation matrix from the frame coordinate system to the coordinate system corresponding to the 3D marker's QR code. These poses are recorded as the poses of the first type of frames. Step S102.2: generating first-category 3D feature points by triangulation based on the pose of the first-category frame; Step S102.3: For images in the map data where the 3D marker is not visible, these are designated as second-category frames. Feature matching is performed using descriptors between the images where the 3D marker is not visible and adjacent images where the 3D marker is visible. Correspondences between the 2D feature points of the images where the 3D marker is not visible and the first 3D feature points are obtained. Based on the corresponding first 3D feature points, the pose of the second-category frames is calculated using the PnP method. Step S102.4: generating second-category 3D feature points by triangulation based on the poses of the second-category frames and the corresponding image information; Step S102.5: Repeat steps S102.1 to S102.2 and S102.3 to S102.4 to obtain more poses of the second type of frames and the second type of three-dimensional feature points, and form an initial static map with the poses of all the first type of frames and the corresponding first type of three-dimensional feature points, and the poses of all the second type of frames and the corresponding second type of three-dimensional feature points.

5. The method for positioning based on visual maps according to claim 2, characterized in that: The performing global map optimization on the initial static map based on the accumulated error to obtain a pre-established static map includes: Nonlinear least squares is used to solve the graph optimization model based on stereo marker pose constraints to obtain the optimal estimated values ​​of the frame pose and the optimal estimated values ​​of the three-dimensional feature points. The pre-established static map is composed of the optimal estimated values ​​of all frame poses and the optimal estimated values ​​of the three-dimensional feature points.

6. The method for positioning based on visual maps according to claim 5, characterized in that: The graph optimization model based on stereoscopic marker pose constraints is calculated as follows: ； Among them, M is a graph optimization model based on the three-dimensional marker pose constraint, and the item to be optimized is and , and yes and The best estimate of The poses of the first type of frames and the poses of the second type of frames are collectively referred to as static map frame poses. Contains all first-class 3D feature points and second-class 3D feature points, collectively referred to as static map 3D feature points. is the reprojection error, To observe the i-th frame pose of the same QR code of the stereo mark, To observe the j-th frame pose of the same QR code of the stereo mark, is the QR code observation error; ； in, for The two-dimensional code observation error between To observe the i-th frame pose of the same QR code of the stereo mark, To observe the j-th frame pose of the same QR code of the stereo mark, The function is an antisymmetric mapping function, is the rotation from the j-th frame pose to the i-th frame pose, is the translation transformation relationship from the j-th frame pose to the i-th frame pose, is the rotation transformation matrix from the i-th frame pose to the world coordinate system, is the translation transformation vector from the i-th frame pose to the world coordinate system, is the rotation transformation matrix from the j-th frame pose to the world coordinate system, is the translation transformation vector from the j-th frame pose to the world coordinate system, The calculation formula is as follows: ； in, is the transformation matrix from the i-th frame pose to the observed QR code coordinate system, is the transformation matrix from the j-th frame pose to the observed QR code coordinate system.

7. The method for positioning based on visual maps according to claim 1, characterized in that: The visual map-based positioning method further includes: When the current frame image is the initial frame and the previous frame image cannot be obtained, the initial positioning method is started; The initial positioning method includes: Step S0.1: Given an initial pose; Step S0.2: Search in the pre-established static map to obtain the map frame closest to the initial pose; Step S0.3: Using a descriptor matching method, match the current frame image with the nearest static map frame, and establish a correspondence between the two-dimensional feature points on the current frame image and the three-dimensional feature points on the static map; Step S0.4: Based on the correspondence between the two-dimensional feature points on the current frame image and the three-dimensional feature points on the static map, the PnP method is used to obtain the initial positioning posture.

8. The method for positioning based on visual maps according to claim 1, characterized in that: The visual map-based positioning method further includes: When the newly acquired image to be positioned exceeds the pre-established static map, temporary three-dimensional feature points are obtained by using the historical frame image and the local static map frame, and the correspondence between the two-dimensional feature points on the current frame image and the temporary three-dimensional feature points is established. Then, the PnP method is used to obtain the positioning pose of the newly acquired image to be positioned; The temporary three-dimensional feature points are obtained as follows: Use frame queue to save current time t to historical time The third type of frame within the time is recorded as a temporary frame; By using the 2D feature matching relationship between the temporary frame and the local static map frame, and the 2D feature matching relationship between each temporary frame, the third type of feature points are obtained through triangulation method and recorded as temporary 3D feature points.

9. The method for positioning based on visual maps according to claim 7, characterized in that: The visual map-based positioning method further includes: Calculate the ratio of the number of 3D feature points that are successfully paired with the descriptor of the current frame image and the descriptor of the local static map frame to the number of corresponding 2D features that are successfully paired; When the ratio is less than the set threshold, the map is considered lost, that is, the image sequence to be located deviates from the pre-established static map. After step S4 is completed, a thread is added, and the previous initial positioning pose is used as the given initial pose, and steps S0.2 to S0.3 are executed, and then step S5 is turned to.

10. A positioning method system based on visual map, characterized in that: include: A data acquisition module, used for acquiring an image sequence to be located; A feature extraction module, configured to extract two-dimensional features and descriptors of each frame of the image in the image sequence to be positioned; A feature matching module is used to match the two-dimensional features of the current frame image with the two-dimensional features of the previous frame image using an optical flow method to obtain matched two-dimensional features; a feature collection module, configured to collect, in a pre-established static map, all static map frames in which the matched two-dimensional features can be observed, and use the collected static map frames as local static map frames, wherein the pre-established static map includes: static map frames having two-dimensional features and descriptors, and three-dimensional feature points corresponding to the static map frames; A temporary map construction module is configured to match the descriptor of the current frame image with the descriptor of the local static map frame using a descriptor matching method, and obtain a temporary map based on the matching results. The temporary map includes: a historical frame image with two-dimensional features and descriptors, and three-dimensional feature points corresponding to the historical frame image; The positioning result acquisition module is used to obtain the optimal estimation results of the current frame image, historical frame image and the three-dimensional feature points corresponding to the historical frame image in the temporary map under the constraints of the pre-established static map, and complete the positioning.

Images