Visual positioning method and system combined with image data

Abstract

The invention relates to the technical field of robot positioning, and discloses a visual positioning method and system combined with image data, and the method comprises the steps: obtaining the motion data of a mobile robot in a motion process; identifying visual marks according to the image data, analyzing visual positioning results, combining the inertial data and the distance data, performing analysis to obtain a first moving track, calculating deviation between the visual positioning result of each visual mark and a corresponding position in the first moving track, and obtaining a position deviation sequence; fusing the visual positioning result and the first moving track, constructing a first pose map, and identifying position failure nodes in the first pose map to obtain a failure node set; removing a failure node and a corresponding connecting edge from the first pose map, analyzing the position of the failure node corresponding to the first moving track, and adding a virtual node to obtain a second pose map; the accuracy and robustness of visual mark failure detection can be improved, and the positioning accuracy of the mobile robot is improved.

Description

Technical Field

[0001] This application relates to the field of robot positioning technology, and more specifically to a visual positioning method and system that combines image data. Background Technology

[0002] High-precision positioning of mobile robots is a prerequisite for ensuring their autonomous task execution. Simultaneous localization and mapping (SLAM) technology is the core solution for robot positioning. Visual positioning is widely used due to its rich information and moderate cost. In practical applications, visual markers are often pre-positioned in the environment. Accurate poses are obtained by recognizing the markers to correct accumulated errors. However, the actual operating environment is complex and variable, and visual markers will be affected by various factors during long-term use.

[0003] The existing technology has the following problems: The confidence level of a marker is determined based on a visual algorithm, but the confidence level output by the visual algorithm has limitations. It passively triggers processing after a positioning failure and cannot respond quickly to positioning failures. Furthermore, visual, inertial, and distance data are processed separately or simply fused, making it impossible to determine the correctness of the data when visual markers fail. For failed visual markers, observation data is directly discarded, leading to information waste and affecting system stability and positioning accuracy. To solve at least one of the above problems, this application proposes a visual positioning method and system that combines image data. Summary of the Invention

[0004] To address the shortcomings of existing technologies, the purpose of this application is to provide a visual positioning method and system that combines image data, effectively solving the problems in the background technology. The specific technical solution of this application is as follows:

[0005] Visual localization methods that combine image data include:

[0006] Acquire motion data during the movement of the mobile robot, including image data, inertial data, and distance data;

[0007] Visual markers are identified based on image data, and the visual positioning results are analyzed. Combined with inertial data and distance data, the first movement trajectory is obtained. The deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory is calculated to obtain the position deviation sequence.

[0008] Based on the position deviation sequence, the visual positioning results and the first movement trajectory are fused to construct the first pose map, identify the position failure nodes in the first pose map, and obtain the set of failure nodes.

[0009] Based on the set of failed nodes, the failed nodes and their corresponding connecting edges are removed from the first pose graph. The positions of the failed nodes corresponding to the first movement trajectory are analyzed, and virtual nodes are added to obtain the second pose graph for localization of the mobile robot.

[0010] Specifically, the process of identifying visual markers based on image data, analyzing the visual positioning results, combining inertial data and distance data to obtain a first movement trajectory, calculating the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory, and obtaining a position deviation sequence includes:

[0011] Visual markers are identified based on image data, and the pixel positions corresponding to the visual markers are extracted to analyze the visual positioning results. Combined with inertial data and distance data, the first movement trajectory is obtained.

[0012] The deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory is calculated to obtain the position deviation sequence.

[0013] Specifically, the step of identifying visual markers based on image data, extracting the pixel positions corresponding to the visual markers to analyze the visual positioning results, and combining inertial data and distance data to analyze and obtain the first movement trajectory includes:

[0014] Visual markers are identified based on image data, and their pixel positions in the image are extracted to obtain a set of pixel positions.

[0015] Based on the set of pixel positions, the rotation matrix and translation vector between the image coordinates and the robot spatial coordinates are calculated, the visual positioning pose is analyzed, and the visual positioning result is obtained.

[0016] The attitude change is obtained by integrating the angular velocity in the inertial data, and the displacement change is obtained by integrating the acceleration in the inertial data. The inertial pose is calculated by combining the attitude change and the displacement change.

[0017] The inertial pose is corrected based on the distance data to obtain the fused pose;

[0018] Arrange the fused poses in chronological order to obtain the first movement trajectory.

[0019] Specifically, the step of calculating the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory to obtain a position deviation sequence includes:

[0020] Transform the visual positioning results and the first movement trajectory to the same coordinate system;

[0021] The positional deviation sequence is obtained by calculating the difference between the visual positioning result of each visual marker in the same coordinate system and the corresponding position in the first movement trajectory.

[0022] Specifically, based on the position deviation sequence, the visual positioning results and the first movement trajectory are fused to construct a first pose map, and the position failure nodes in the first pose map are identified to obtain a set of failure nodes, including:

[0023] Based on the position deviation sequence, the visual positioning result and the first movement trajectory are fused. The visual positioning pose corresponding to the visual positioning result is used as the marker node, and the fused pose in the first movement trajectory is used as the metric node. A connection edge is established between the marker node and the metric node to construct the first pose graph.

[0024] Calculate the deviation between the marked node and the corresponding metric node, identify the position failure nodes in the first pose graph, and obtain the set of failure nodes.

[0025] Specifically, the step of fusing visual positioning results and a first movement trajectory based on the position deviation sequence, using the corresponding visual positioning pose in the visual positioning results as a marker node, and the fused pose in the first movement trajectory as a metric node, and establishing connection edges between the marker nodes and the metric nodes to construct the first pose graph includes:

[0026] Based on the multiple visual positioning poses and position deviation sequences corresponding to each visual marker, the visual positioning pose with the smallest position deviation is selected as the marker node, thus obtaining the marker node sequence.

[0027] The fused pose at the time corresponding to the visual marker in the first movement trajectory is used as the metric node to obtain the metric node sequence;

[0028] For each labeled node in the labeled node sequence, select the adjacent preceding and following metric nodes from the metric node sequence and establish the first connecting edge;

[0029] For adjacent metric nodes in the metric node sequence, a second connection edge is established by calculating the relative pose transformation;

[0030] By combining the marker node, the metric node, the first connecting edge, and the second connecting edge, the first pose graph is constructed.

[0031] Specifically, the deviation between the calculated marker node and the corresponding metric node is used to identify the position failure nodes in the first pose graph, resulting in a set of failure nodes, including:

[0032] Calculate the deviation between the marked node and the corresponding connection metric node to obtain the first deviation value;

[0033] By combining historical position deviation sequence analysis with the position deviation of the marked nodes, the second deviation value is obtained;

[0034] By combining the first deviation value and the second deviation value, the position failure nodes in the first pose graph are identified, and the set of failure nodes is obtained.

[0035] Specifically, according to the set of failed nodes, the failed nodes and their corresponding connecting edges are removed from the first pose graph, the positions of the failed nodes corresponding to the first movement trajectory are analyzed, and virtual nodes are added to obtain the second pose graph, including:

[0036] Based on the set of failed nodes, remove the failed nodes and their corresponding connecting edges from the first pose graph to obtain the updated pose graph;

[0037] Based on the updated pose graph, the positions of the failed nodes corresponding to the first movement trajectory are analyzed, virtual nodes are added, and the second pose graph is obtained.

[0038] Specifically, the step of analyzing the position of the failed node corresponding to the first movement trajectory based on the updated pose graph, adding virtual nodes, and obtaining the second pose graph includes:

[0039] Based on the updated pose graph, the position of the failure node corresponding to the first moving trajectory is analyzed, and the trajectory fragments of the failure node in the first moving trajectory are extracted to obtain a set of trajectory fragments.

[0040] The first relative pose is obtained by integrating each trajectory segment in the trajectory segment set, and the second relative pose is obtained by calculating the pose difference between adjacent metric nodes connected to the failed node.

[0041] By combining the first relative pose and the second relative pose, a virtual pose of the virtual node is constructed and added to the corresponding failed node position to obtain the second pose graph.

[0042] A visual positioning system combining image data, used to implement the aforementioned visual positioning method combining image data, includes:

[0043] The data acquisition module acquires motion data during the movement of the mobile robot, including image data, inertial data, and distance data.

[0044] The position analysis module identifies visual markers based on image data and analyzes the visual positioning results. Combining inertial data and distance data, it analyzes and obtains the first movement trajectory, calculates the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory, and obtains the position deviation sequence.

[0045] The failure detection module, based on the position deviation sequence, fuses the visual positioning results and the first movement trajectory to construct the first pose map, identifies the position failure nodes in the first pose map, and obtains the set of failure nodes;

[0046] The robot localization module removes the failed nodes and their corresponding connecting edges from the first pose graph according to the set of failed nodes, analyzes the position of the failed nodes corresponding to the first movement trajectory, adds virtual nodes, and obtains the second pose graph to locate the mobile robot.

[0047] The beneficial effects of this application are as follows: By calculating the positional deviation sequence between the visual positioning result and the trajectory, anomalies in visual markers can be identified, improving detection accuracy; by analyzing marker nodes and metric nodes and establishing connecting edges, a first pose graph is constructed, providing a structural reference for the failure identification of visual markers; for the detection of failed nodes, by combining the deviation between the current marker node and the metric node with the historical positional deviation sequence for analysis, misjudgment caused by single random errors can be effectively avoided, improving the accuracy and robustness of failure detection; by analyzing the trajectory segment corresponding to the failed node in the first moving trajectory and combining the pose information between adjacent metric nodes, a virtual node is constructed for positioning analysis, enabling smooth transition and downgraded positioning at the failure moment, thus improving the positioning accuracy of the mobile robot. Attached Figure Description

[0048] Figure 1 This is a flowchart illustrating the visual localization method combining image data in the embodiments of this application.

[0049] Figure 2 This is a flowchart illustrating the process of constructing the first pose graph in an embodiment of this application.

[0050] Figure 3 This is a schematic diagram of the structure of the visual positioning system combining image data in an embodiment of this application. Detailed Implementation

[0051] The present application will be further described in detail below with reference to the accompanying drawings and embodiments.

[0052] In the embodiments of this application, the terms "exemplary" or "for example" are used to indicate that something is an example, illustration, or description. Any embodiment or design that is described as "exemplary" or "for example" in the embodiments of this application should not be construed as being more preferred or advantageous than other embodiments or design. Specifically, the use of the terms "exemplary" or "for example" is intended to present the relevant concepts in a specific manner.

[0053] Hereinafter, the terms "first," "second," and other generic terms are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0054] refer to Figure 1 The image shown illustrates a specific implementation of the visual positioning method combining image data according to this application, including:

[0055] S101. Acquire motion data during the movement of the mobile robot, wherein the motion data includes image data, inertial data, and distance data;

[0056] S102. Identify visual markers based on image data and analyze the visual positioning results. Combine inertial data and distance data to obtain the first moving trajectory. Calculate the deviation between the visual positioning result of each visual marker and the corresponding position in the first moving trajectory to obtain the position deviation sequence.

[0057] S103. Based on the position deviation sequence, the visual positioning results and the first movement trajectory are fused to construct the first pose map, identify the position failure nodes in the first pose map, and obtain the set of failure nodes.

[0058] S104. According to the set of failed nodes, remove the failed nodes and their corresponding connecting edges from the first pose graph, analyze the position of the failed nodes corresponding to the first movement trajectory, add virtual nodes, and obtain the second pose graph to locate the mobile robot.

[0059] In this embodiment, during the mobile robot's task execution, multiple onboard sensors continuously collect environmental and self-motion information, including image data, inertial data, and distance data. Image data is acquired by a monocular or binocular camera, obtaining grayscale or color images of the robot's forward direction at a rate of 15 to 30 frames per second. This data is used to identify pre-placed visual markers in the environment, including but not limited to QR codes, April Tags, or custom topological markers. Inertial data is acquired by an inertial measurement unit (IMU), which includes a three-axis accelerometer and a three-axis gyroscope, outputting raw values ​​of the robot's acceleration and angular velocity at a high frequency of 100Hz to 500Hz. Distance data is obtained by a lidar system that emits laser beams and receives reflected signals to acquire point cloud distance information of surrounding obstacles. The acquired data is precisely timestamped and time-aligned by the robot's operating system, providing a data foundation for robot localization analysis.

[0060] Preferably, the inertial data processing includes angular velocity integration and acceleration integration. Angular velocity integration is to accumulate the instantaneous angular velocity value output by the gyroscope over time to obtain the pitch angle, roll angle, and yaw angle, which correspond to the changes in the robot's attitude. Acceleration integration is to subtract the gravity component from the instantaneous acceleration value output by the accelerometer and then integrate it twice over time to obtain the displacement change of the robot's position.

[0061] It should be noted that by simultaneously acquiring image data, inertial data, and distance data, image data provides environmental semantic features, inertial data ensures high-frequency motion tracking, and distance data provides geometric constraints. Through data fusion, the positioning capability of visual markers can be utilized, and the inertial and distance data can be fused to obtain short-term, high-precision relative motion trajectories, thereby improving the accuracy of visual marker failure detection and avoiding the problem of single sensors being prone to failure in complex environments.

[0062] Furthermore, visual marker recognition processing is performed on the acquired image frames. Taking QR codes or April Tags as examples, the detection algorithm performs adaptive threshold binarization on the image, converting the grayscale image into a black and white binary image; connected component analysis is performed to extract candidate quadrilateral regions; the internal encoding matrix is ​​decoded to confirm whether it is a valid marker, and the pixel coordinates of the four corner points of the marker in the image are extracted; using the correspondence between the two-dimensional pixel coordinates and the known physical size of the marker, the rotation matrix and translation vector between the camera coordinate system and the marker coordinate system are calculated by solving the perspective n-point problem, and the visual positioning pose of the robot at the moment of detecting the marker is obtained as the visual positioning result.

[0063] Simultaneously, the inertial data is processed. The angular velocity output from the gyroscope is integrated to obtain the real-time change in the robot's attitude; the acceleration output from the accelerometer is integrated and the gravitational component is subtracted to obtain the displacement change. The attitude change and displacement change are combined to obtain the inertial pose. The inertial pose suffers from integral drift, which is corrected using distance data. A laser point cloud matching algorithm is used to calculate the precise pose transformation between adjacent time steps. This transformation is used as the observation value, and the inertial pose is updated by an extended Kalman filter to obtain the fused pose. The fused poses from consecutive time steps are arranged in chronological order to obtain the first movement trajectory.

[0064] Specifically, the visual localization results and the first movement trajectory are transformed to the same global coordinate system, which can be the initial position coordinate system when the robot starts. For each detected visual marker, the position coordinates in the visual localization result and the fused position coordinates in the first movement trajectory at the same moment are extracted, and the Euclidean distance difference between the two is calculated to obtain a numerical value. This process is repeated for all markers to obtain a set of deviation values ​​arranged in time, which serves as the position deviation sequence. By constructing the deviation sequence between visual marker localization and the fused trajectory, failure markers of the visual markers can be effectively identified.

[0065] Preferably, the perspective n-point problem is a method for solving camera pose using n three-dimensional spatial points and their two-dimensional projection points on an image. Its inputs are the three-dimensional physical coordinates of the marked corner points and the detected two-dimensional pixel coordinates, and the output is the six-DOF pose of the camera relative to the marked points. Extended Kalman filtering is a recursive state estimation method used to fuse high-frequency inertial data and low-frequency but accurate laser / odometry data. Its state variables include robot position, velocity, attitude, and IMU zero-bias error. The prediction step updates the state and covariance using IMU integration, and the update step calculates the Kalman gain and corrects the state using pose observations provided by the lidar or odometry, outputting the optimal estimated fused pose.

[0066] Specifically, a first pose graph is constructed based on the position deviation sequence. This first pose graph includes marker nodes and metric nodes. Marker nodes correspond to the visual localization pose of each visual marker, and metric nodes correspond to the fused pose at each moment in the first motion trajectory. Marker nodes are selected by filtering them. Since the same physical marker is observed continuously across multiple frames, generating multiple visual localization results, the one with the smallest deviation from the first motion trajectory is chosen as the representative marker node, resulting in a marker node sequence. Metric nodes are directly selected from the fused pose at the corresponding moment of the visual marker, resulting in a metric node sequence. Connection edges are established. The first connection edge connects a marker node to its two temporally nearest metric nodes, constraining the position of the marker node relative to the robot's motion trajectory. The second connection edge connects adjacent metric nodes, with its edge weight calculated from the relative pose transformation between the two moments, constraining the continuity and smoothness of the robot's motion.

[0067] After constructing the first pose map, failed nodes are identified. For each marked node, the average deviation between it and the preceding and following measurement nodes is calculated to obtain a first deviation value, reflecting the instantaneous consistency of the mark within the current local range. Simultaneously, the historical position deviation sequence corresponding to the mark is retrieved, and the deviation is analyzed to determine whether it exhibits a gradual increase or abrupt change, yielding a second deviation value. The first and second deviation values ​​are combined for judgment. For example, if the first deviation value exceeds a preset deviation threshold (set according to the system's positioning accuracy requirements, such as 0.1 meters), and the second deviation value shows that the historical deviation of the mark is continuously increasing or exhibits abrupt changes, then the marked node is determined to be a position failed node. All marked nodes determined to be failed are integrated to obtain a set of failed nodes.

[0068] It should be noted that by constructing a pose graph containing two types of nodes and combining instantaneous deviations and historical trends to determine failed nodes, failure markers can be accurately identified, effectively avoiding misjudgments caused by noise in a single frame image or temporary occlusion, thus improving the accuracy and robustness of failure detection. By identifying failure markers, a structural basis is provided for subsequent localization analysis and compensation processes, thereby improving the accuracy of localization results.

[0069] Based on the set of failed nodes, all nodes marked as failed and all connected edges to them are removed from the first pose graph to obtain the updated pose graph. At this point, gaps appear in the positions originally occupied by failed nodes, causing discontinuities in the graph structure. To fill these gaps, virtual nodes are added. Based on the updated pose graph, the first movement trajectory is traced back, and trajectory segments near the time corresponding to the failed nodes are extracted to obtain a set of trajectory segments. For each failed node, its corresponding trajectory segment is integrated to calculate the total displacement and rotation changes within that segment, obtaining the first relative pose. The pose difference between two metric nodes adjacent to the failed node in the updated pose graph is calculated to obtain the second relative pose.

[0070] Specifically, using the first relative pose as the basis for the motion trend and the second relative pose as the global consistency anchor point, an optimal virtual pose is solved through a nonlinear optimization method. The virtual node is inserted into the position of the original failed node, and a new connection edge is established between it and the adjacent metric node to obtain the second pose graph. Based on the second pose graph containing the virtual node, the robot's final localization result is output.

[0071] It should be noted that by removing failed nodes and adding virtual nodes, the impact of erroneous observations on the localization results can be eliminated, avoiding trajectory interruptions caused by data removal. By constructing virtual nodes by combining local motion history and global proximity constraints, the resulting pose not only conforms to the robot's actual motion trend at the time, but also remains consistent with reliable observations in the surrounding area, ensuring the smoothness and accuracy of the localization output results, improving the accuracy and robustness of the second pose graph, and enabling continuous, stable, and reliable localization services for mobile robots in environments where some visual markers fail, thereby enhancing the system's adaptability under complex working conditions.

[0072] This application identifies anomalies in visual markers and improves detection accuracy by calculating the positional deviation sequence between the visual positioning result and the trajectory. It constructs a first pose graph by analyzing marker nodes and metric nodes and establishing connecting edges, providing a structural reference for visual marker failure identification. For the detection of failed nodes, analysis combining the deviation between the current marker node and metric node with historical positional deviation sequences effectively avoids misjudgments caused by single random errors, improving the accuracy and robustness of failure detection. By analyzing the trajectory segments corresponding to failed nodes in the first moving trajectory and combining the pose information between adjacent metric nodes, a virtual node is constructed for positioning analysis, enabling smooth transition and downgraded positioning at the failure moment, thus improving the positioning accuracy of the mobile robot.

[0073] Furthermore, visual markers are identified based on image data, and the visual positioning results are analyzed. Combined with inertial data and distance data, a first movement trajectory is obtained. The deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory is calculated to obtain a position deviation sequence, including:

[0074] S201. Identify visual markers based on image data, extract the pixel positions corresponding to the visual markers, analyze the visual positioning results, and combine inertial data and distance data to obtain the first movement trajectory.

[0075] S202. Calculate the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory to obtain the position deviation sequence.

[0076] In this embodiment, visual markers are identified based on image data, and the pixel positions corresponding to the visual markers are extracted to analyze the visual positioning results. Combined with inertial data and distance data, a first movement trajectory is obtained. By processing the visual positioning path and the fusion path of inertial and distance data in parallel, two independent positioning information is obtained. Visual positioning provides absolute pose information based on environmental features, which has the advantage of no cumulative drift, but is easily affected by ambient lighting and marker visibility. The fusion of inertial and distance data provides relative trajectory information based on kinematic recursion, which has the characteristics of high frequency and continuity, but suffers from long-term drift problems. The two positioning information can be used as a benchmark to verify the reliability of the visual markers, realize the complementarity between sensor information, and improve the accuracy of visual positioning. An extended Kalman filter is used in the fusion path to effectively suppress the drift error of the inertial sensor, so that the first movement trajectory has high accuracy within a short time window, providing an accurate data basis for subsequent deviation calculation.

[0077] Specifically, the deviation between the visual positioning result of each visual marker and its corresponding position in the first moving trajectory is calculated to obtain a position deviation sequence. By constructing the deviation sequence between the visual positioning result and the first moving trajectory, accurate data support is provided for subsequent failure detection, effectively identifying the failure state of visual markers. By establishing a deviation sequence in the time dimension, the instantaneous deviation value of a single observation and the trend of deviation change over time can be obtained. The deviation sequence reflects the reliability of the visual marker under the current environmental conditions. Reducing the number of visual markers with large deviations can improve positioning accuracy and precision.

[0078] Furthermore, visual markers are identified based on image data, and the pixel positions corresponding to the visual markers are extracted to analyze the visual positioning results. Combined with inertial data and distance data, the first movement trajectory is obtained, including:

[0079] S301. Identify visual markers based on image data, extract the pixel positions of the visual markers in the image, and obtain a set of pixel positions;

[0080] S302. Based on the set of pixel positions, calculate the rotation matrix and translation vector between the image coordinates and the robot spatial coordinates, analyze the visual positioning pose, and obtain the visual positioning result.

[0081] S303. Integrate the angular velocity in the inertial data to obtain the attitude change, integrate the acceleration in the inertial data to obtain the displacement change, and calculate the inertial pose by combining the attitude change and displacement change.

[0082] S304. Correct the inertial pose based on the distance data to obtain the fused pose;

[0083] S305. Arrange the fused poses in chronological order to obtain the first movement trajectory.

[0084] In this embodiment, during the operation of the mobile robot, the onboard industrial camera acquires images of the surrounding environment at a fixed frame rate, which can be set to 30 frames per second. Each frame of the image undergoes adaptive threshold binarization processing. The adaptive threshold binarization processing dynamically calculates the segmentation threshold based on the grayscale distribution of the local region of the image, converting the grayscale image into a black and white binary image to eliminate the influence of uneven lighting. Connectivity analysis is performed on the binary image, and all closed contours are extracted by scanning the pixel neighborhood. A polygon approximation algorithm is then used to select quadrilateral regions as candidate markers.

[0085] For each candidate marker's quadrilateral region, perspective transformation is used to correct it into a square, and the internal encoding region is decoded. Taking a QR code as an example, the encoding region consists of black and white cells. By sampling the grayscale value of each cell and matching it with a preset encoding dictionary, the marker's identity is determined. After successful decoding, the gradient direction fitting method is used to extract the sub-pixel coordinates of the marker's four corner points in the original image. The gradient direction fitting method includes: performing quadratic curve fitting along the edge gradient direction near the initial integer position of the corner point, calculating the decimal offset of the extreme point, and obtaining accurate corner coordinates. The coordinates of the four corner points of all detected markers and their corresponding marker identities are integrated to obtain the pixel position set of the frame image. If multiple visual markers are detected in a frame image, the pixel position set includes multiple sets of data; if no markers are detected, the pixel position set is empty.

[0086] It should be noted that the adaptive threshold binarization algorithm is a dynamic thresholding segmentation method. Its input is the original grayscale image, and its output is a binary image. For each pixel in the image, the algorithm uses the average grayscale value or Gaussian weighted average value of pixels within its neighborhood window as the segmentation threshold. If the current pixel's grayscale value is higher than this threshold, it is set to white; otherwise, it is set to black. This effectively handles local lighting changes such as shadows and reflections. Connected component analysis uses a two-pass scanning method. The first pass sets a temporary label for each pixel and records equivalence relationships. The second pass merges the equivalence labels, outputting the bounding rectangle and contour point set for each connected component. The polygon approximation algorithm uses the Douglas-Puk algorithm, recursively extracting key vertices from the contour point set. If the simplified contour has four vertices and good convexity, it is considered a candidate label. The corner detection algorithm is based on image gradient information. The input is integer pixel corner coordinates and their neighborhood image window. Sub-pixel offsets are obtained by solving for the extrema in the local gradient direction, and the output is floating-point representation of the corner coordinates.

[0087] Preferably, by combining adaptive threshold binarization with corner detection technology, the robustness and accuracy of visual marker detection are improved; adaptive threshold processing can stably extract marker contours under warehouse lighting changes, local shadows or reflections, avoiding missed or false detections caused by fixed thresholds; corner detection can improve corner positioning accuracy, reduce pose noise introduced by corner quantization errors, and improve the accuracy and precision of failure detection.

[0088] Specifically, the prior 3D information of each visual marker is obtained, including the spatial coordinates of the four corner points in the marker coordinate system. Four pairs of 2D-3D corresponding points are formed by combining the four 2D pixel coordinates of the same marker with the spatial coordinates of these four known 3D points. Using these four pairs of corresponding points as input, the perspective n-point problem solving algorithm is called to calculate the rotation matrix and translation vector between the camera coordinate system and the marker coordinate system. The perspective n-point problem solving algorithm constructs the projection equation between the 3D point and the 2D projection point, obtains an initial solution using direct linear transformation or an efficient perspective n-point problem solver, and minimizes the reprojection error of all corresponding points using the Levenberg-Marquardt nonlinear optimization algorithm to obtain the optimal rotation matrix and translation vector. The rotation matrix describes the pose transformation from the marker coordinate system to the camera coordinate system, and the translation vector describes the position of the origin of the marker coordinate system in the camera coordinate system.

[0089] After obtaining the camera's pose relative to the marker, and combining this with the pre-calibrated extrinsic parameters of the camera's installation on the robot body—namely, the fixed rotation matrix and translation vector from the camera coordinate system to the robot's central coordinate system—the robot's pose in the marker coordinate system is calculated through a coordinate transformation chain. If the marker itself has known global map coordinates, the robot's pose is further transformed into the global coordinate system, outputting the visual localization pose at that moment. This visual localization result is represented in six-dimensional pose form, including three-dimensional position coordinates (x, y, z) and three-dimensional attitude angles (roll, pitch, yaw), along with a timestamp and corresponding marker identification.

[0090] It should be noted that the perspective n-point problem solving algorithm is a standard method in computer vision used to estimate camera pose from 2D-3D corresponding points. Its input is n sets of 3D spatial point coordinates and their corresponding 2D image pixel coordinates, and its output is the rotation matrix R and translation vector t of the camera coordinate system relative to the marker coordinate system. The efficient perspective n-point problem solver is a non-iterative method that transforms the problem into solving for the coordinates of the control points in the camera coordinate system by representing the 3D point as a weighted sum of four virtual control points. It is computationally fast and highly accurate. The Levenberg-Marquardt algorithm is a non-linear optimization method combining the Gauss-Newton method and gradient descent to minimize reprojection error. Its input is the initial R, t, and all corresponding points. It iteratively adjusts R and t to minimize the sum of the Euclidean distances between the projected points of all 3D points and the detected pixels, outputting the optimized and accurate R and t. The reprojection error threshold is typically set to 0.5 pixels or 1 pixel. If the average error after optimization exceeds this threshold, the solution is considered unreliable and the visual localization result is discarded.

[0091] Preferably, by employing a high-efficiency perspective n-point problem solver combined with nonlinear optimization, high-precision positioning results can be obtained while ensuring real-time performance, meeting the positioning accuracy requirements of industrial robots. The introduction of a coordinate transformation chain compensates for deviations in camera mounting position and angle, allowing for accurate calculation of the robot's pose regardless of whether the camera is mounted on top, in front, or to the side of the robot. If global map coordinates have been pre-established in the environment, the robot's absolute pose in the global coordinate system can be directly output, eliminating accumulated errors. By controlling the reprojection error threshold, the consistency of visual positioning results is improved, providing accurate data support for constructing a deviation sequence.

[0092] For angular velocity integration, zero-bias compensation is applied to the angular velocity output by the gyroscope, and a numerical integration method is used to calculate the attitude change between two adjacent sampling moments. Specifically, the average of the current angular velocity measurement and the previous angular velocity measurement is calculated as the average angular velocity within this time interval, and multiplied by the time interval to obtain the angle increment within this time interval. This angle increment is converted into a quaternion or rotation matrix form and multiplied by the attitude quaternion from the previous moment to obtain the attitude quaternion at the current moment, thus updating the robot's attitude information.

[0093] For acceleration integration, the gravitational component is subtracted from the accelerometer output, and the accelerometer zero bias is subtracted to obtain the robot's linear acceleration. The linear acceleration is integrated once to obtain the velocity change, which is accumulated to the velocity at the previous moment to obtain the current velocity. The velocity is then integrated twice to obtain the displacement change, which is accumulated to the position at the previous moment to obtain the current position. Since acceleration integration introduces error accumulation, pre-integration is used: the inertial measurement data over a period of time is processed as a whole to calculate the relative motion increment during that period, including relative rotation, relative velocity, and relative position, and the corresponding covariance matrix is ​​calculated. When performing only pure inertial recursion, the above integration operation is continuously executed, updating the robot's inertial pose once for each frame of inertial data received, resulting in a high-frequency inertial trajectory.

[0094] Preferably, the motion estimation path is obtained through high-frequency inertial data integration, ensuring the continuity of robot localization during periods of brief absence of visual markers or image processing delays. Inertial pose has high-frequency output characteristics, enabling the capture of the robot's rapid movements and attitude changes, providing rich prior motion information for subsequent sensor fusion. Acceleration integration, after gravity subtraction and zero-bias compensation, can suppress low-frequency drift and improve the accuracy of the inertial trajectory over short periods.

[0095] Specifically, when two consecutive frames of point cloud data are received, an iterative nearest-point algorithm is used for point cloud registration to calculate the robot's relative pose transformation between the two frames. An extended Kalman filter is used for data fusion. The filter's state vector is designed to include the robot's current position, velocity, attitude, and the zero bias of the inertial measurement unit's accelerometer and gyroscope. The filtering process is divided into two stages: prediction and update. In the prediction stage, whenever new inertial data is received, the state vector is further predicted through inertial integration, and the state covariance matrix is ​​updated based on the inertial measurement noise characteristics.

[0096] During the update phase, upon receiving relative pose observations from the lidar, an observation equation is constructed: a relationship exists between the observed value and the predicted value of the current state. A predicted relative pose is calculated using the difference between the current state and the state at the previous moment, and this predicted pose is compared with the lidar observation to obtain innovation. The Kalman gain is calculated based on the innovation and the observation noise covariance. This Kalman gain is then used to weight and correct the predicted state, yielding the optimal estimated posterior state, i.e., the fused current pose. Simultaneously, the state covariance matrix is ​​updated. The observation noise covariance matrix in the filter can be dynamically adjusted based on the lidar's ranging accuracy and registration residuals. For example, when the registration score is low, the observation noise is increased, and the weight of that observation is decreased.

[0097] It should be noted that the Iterative Closest Point Algorithm (IBPA) is a classic point cloud registration method. Its input consists of two frames of point cloud data and an initial relative pose estimate, and its output is the precise relative pose transformation. The algorithm iteratively finds the closest point pairs between two frames of point cloud data, minimizing the sum of squared Euclidean distances between all point pairs. The optimal transformation is obtained after iterative convergence. The state prediction model is an inertial integral, and the state update model is based on lidar relative pose observations; both exhibit nonlinear relationships. The filter achieves linearization by calculating the Jacobian matrix, and the optimal state is solved recursively.

[0098] Preferably, an extended Kalman filter is used to fuse high-frequency inertial data with low-frequency, high-precision distance data. The inertial data ensures the high-frequency continuity of the pose output and can fill in the motion details within the LiDAR sampling interval; the LiDAR data can effectively suppress the long-term drift of the inertial integral, enabling the fused pose to maintain high local accuracy over long-term operation. The covariance dynamic adjustment process in the filter can adaptively adjust the fusion weights according to the observation quality, improving the robustness of the system and providing an accurate data basis for the subsequent deviation calculation of the visual positioning results.

[0099] Specifically, the system maintains a trajectory data container internally, such as a circular buffer, list, or database table, to store historical fused poses. Whenever the extended Kalman filter outputs a new fused pose, the system immediately adds that pose and its precise timestamp to the end of the trajectory data container. Each element in the container contains six dimensions of pose information and a corresponding timestamp. Trajectories can be queried by time: given a specific moment, the fused pose at that moment can be quickly returned. Arranging the fused poses in chronological order yields the first movement trajectory. By integrating the fused poses to obtain the first movement trajectory, the system can reflect the robot's continuous motion process and provide reliable relative position estimation.

[0100] Furthermore, the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory is calculated to obtain a position deviation sequence, including:

[0101] S401. Convert the visual positioning result and the first movement trajectory to the same coordinate system;

[0102] S402. Calculate the position coordinate difference between the visual positioning result of each visual marker in the same coordinate system and the corresponding position in the first movement trajectory to obtain the position deviation sequence.

[0103] In this embodiment, the visual positioning result is typically defined in a global coordinate system referenced by a visually marked map, such as a warehouse coordinate system or a factory coordinate system. The origin of this coordinate system is set at a fixed location, such as the warehouse entrance, and the coordinate axes are aligned with the building structure. The first movement trajectory is defined in the initial coordinate system at the moment of robot startup, with its origin at the robot's starting position and the coordinate axes related to the robot's initial orientation. A fixed spatial transformation relationship exists between these two coordinate systems, which needs to be unified through calibration and coordinate transformation.

[0104] Specifically, the transformation parameters between two coordinate systems are determined by observing visual markers at multiple known locations. When the robot moves in the environment and observes a visual marker, the localization result of the marker in the visual global coordinate system is obtained in step S302. Simultaneously, the fused pose of the robot in the initial coordinate system at the same moment is obtained from the first movement trajectory. Since the visual marker itself has precise coordinates in the visual global coordinate system, and the robot's pose in the initial coordinate system is known, the rotation matrix and translation vector from the initial coordinate system to the visual global coordinate system are solved by constructing these two sets of correspondences. Observation data of markers at multiple different locations are collected, an overdetermined system of equations is constructed, and the optimal transformation parameters are solved using the least squares method.

[0105] After obtaining the coordinate system transformation parameters, coordinate transformation is performed for each subsequent visual localization result and each fused pose. When it is necessary to transform the visual localization result to the coordinate system of the first motion trajectory, the coordinate vector of the visual localization result is multiplied by the rotation matrix and then added to the translation vector to obtain the transformed coordinates; if it is necessary to transform the first motion trajectory to the visual global coordinate system, the inverse transformation is performed.

[0106] It should be noted that the rotation matrix is ​​a 3x3 orthogonal matrix describing the attitude rotation relationship between two coordinate systems; the translation vector is a three-dimensional vector describing the positional offset between the origins of the two coordinate systems. The least squares method estimates the optimal transformation parameters by minimizing the sum of squared errors. In this embodiment, it is used to solve for the optimal rotation matrix and translation vector from multiple sets of corresponding points. In specific implementation, the singular value decomposition method can be used to solve the problem: first, the centroids of the two sets of points are calculated to decenter the point sets; then, the covariance matrix is ​​constructed and singular value decomposition is performed; the rotation matrix is ​​directly obtained from the decomposition result; and then the translation vector is calculated based on the rotation matrix and the centroid coordinates.

[0107] Preferably, by using coordinate system one, the problem of data incomparability caused by different reference benchmarks can be eliminated. By using the least squares method to fuse multiple marker observations to solve the transformation parameters, random errors in single calibration observations can be effectively suppressed, the reliability of the transformation relationship can be improved, and systematic deviations can be avoided due to calibration errors. The unified coordinate system allows the visual positioning results and the fused trajectory to be directly compared in the same spatial framework, and the deviation calculation results reflect the degree of geometric deviation between the visual markers and the actual robot motion.

[0108] Specifically, all visual localization results are iterated. For each visual localization result, based on its timestamp, the fused pose at the same or nearest moment is found from the first movement trajectory. After obtaining the visual localization position coordinates and the fused trajectory position coordinates at the same moment, the Euclidean distance between them is calculated, reflecting the position deviation of the visual marker at the current moment. For each successful visual marker detection, the above calculation is performed, and the result is recorded as a data unit. This data unit includes the visual marker's identifier, the timestamp of the detection moment, and the calculated position deviation value. As the robot continues to move, new visual markers are constantly detected, generating new deviation data units. These deviation data units are organized and integrated in chronological order to obtain the position deviation sequence.

[0109] Preferably, the data in the position deviation sequence reflects the consistency of the visual marker's positioning at different times. A sliding window statistical processing is performed on the position deviation sequence to calculate the average deviation, deviation variance, or deviation change rate within the sliding window. The sliding window length can be set to, for example, 10 detections or a 30-second time window.

[0110] It should be noted that by quantifying the deviation between the visual marker localization and the robot's actual motion trajectory, accurate data support is provided for subsequent failure detection. This can effectively identify the failure state of the visual marker, and the deviation sequence can also be used to analyze the trend of deviation changes over time. By recording the deviation sequence for each visual marker, different processing strategies can be adopted for different markers, thereby improving the accuracy and robustness of the localization system.

[0111] Furthermore, based on the position deviation sequence, the visual positioning results and the first movement trajectory are fused to construct a first pose map. Position failure nodes in the first pose map are identified, resulting in a set of failure nodes, including:

[0112] S501. Based on the position deviation sequence, fuse the visual positioning result and the first moving trajectory, take the corresponding visual positioning pose in the visual positioning result as the marker node, take the fused pose in the first moving trajectory as the metric node, establish the connection edge between the marker node and the metric node, and construct the first pose graph.

[0113] S502. Calculate the deviation between the marked node and the corresponding metric node, identify the position failure node in the first pose graph, and obtain the set of failure nodes.

[0114] In this embodiment, based on the position deviation sequence, the visual positioning result and the first movement trajectory are fused. The corresponding visual positioning pose in the visual positioning result is used as a marker node, and the fused pose in the first movement trajectory is used as a metric node. A connection edge is established between the marker node and the metric node to construct the first pose graph. By constructing the first pose graph containing two types of nodes and connection edges, the marker node and the metric node enable the visual observation and motion trajectory to be expressed independently. The absolute positioning information of the visual marker and the relative motion information fused from the inertial data and distance data are preserved. The first connection edge directly connects the marker node and the metric node, so that the positioning result of each visual marker is anchored within a specific time period of the robot's actual motion trajectory. The second connection edge ensures that even if some marker nodes fail, the overall structure of the graph remains stable, improving the robustness of the positioning system.

[0115] Specifically, the deviation between the marked node and the corresponding metric node is calculated to identify the position failure nodes in the first pose graph, thus obtaining a set of failure nodes. Combining instantaneous deviation with historical trends for failure detection improves the robustness of the system. By constructing the set of failure nodes, an accurate structural foundation is provided for subsequent graph reconstruction optimization and degraded localization, thereby improving the accuracy and robustness of the localization results.

[0116] like Figure 2As shown, based on the position deviation sequence, the visual positioning result and the first movement trajectory are fused. The corresponding visual positioning pose in the visual positioning result is used as a marker node, and the fused pose in the first movement trajectory is used as a metric node. A connection edge is established between the marker node and the metric node to construct the first pose graph, including:

[0117] S601. Based on the multiple visual positioning poses and position deviation sequences corresponding to each visual marker, select the visual positioning pose with the smallest position deviation as the marker node to obtain the marker node sequence.

[0118] S602. Use the fused pose at the time corresponding to the visual marker in the first moving trajectory as the metric node to obtain the metric node sequence;

[0119] S603. For each labeled node in the labeled node sequence, select the adjacent preceding and following metric nodes from the metric node sequence and establish the first connecting edge.

[0120] S604. For adjacent metric nodes in the metric node sequence, establish a second connection edge by calculating the relative pose transformation;

[0121] S605. Combine the marker node, metric node, first connecting edge, and second connecting edge to construct the first pose graph.

[0122] In this embodiment, during actual operation, when the mobile robot passes near a visual marker, due to the continuity of the camera frame rate and the influence of the robot's movement speed, the same physical marker is often captured continuously in multiple frames. Each frame calculates a visual positioning pose, and all deviation data related to the marker's identity are extracted from the position deviation sequence. The position deviation sequence records the time corresponding to each visual marker detection, the marker's identity, and the position deviation value between the visual positioning result of that detection and the first movement trajectory.

[0123] For the same marker identity, all its historical detection records are collected to form a candidate list. Each element in the list contains the detection time, visual positioning pose, and corresponding position deviation value. The candidate list is traversed, and the position deviation value of each candidate is compared. The detection record with the smallest position deviation value is found. The smallest deviation value indicates that the visual positioning result best matches the robot's actual motion trajectory, meaning the observation is least affected by noise and has the highest positioning accuracy. The visual positioning pose corresponding to this detection is selected as the marker node for that marker and added to the marker node sequence. The marker node sequence is an ordered list arranged according to the time sequence in which the marker was first detected. Each node contains the marker identity, the selected visual positioning pose, the corresponding time, and the position deviation value of that pose.

[0124] It should be noted that by selecting the best among multiple observations of the same label, the formation of dense local constraint clusters by multiple observations of the same label in graph optimization is effectively avoided. The observation with the smallest deviation is selected as the label node, which improves the quality of the input data. The establishment of the label node sequence provides an accurate node index for the subsequent establishment of connection edges. Each label has one and only one representative node in the graph.

[0125] Specifically, the first motion trajectory is a sequence of continuously fused poses arranged in chronological order. The time corresponding to each marker node in the marker node sequence is obtained. For each marker node, the corresponding fused pose is found in the first motion trajectory. For each marker node in the marker node sequence, after obtaining the corresponding fused pose, it is defined as a metric node. The metric nodes are arranged in the order of their corresponding marker nodes to obtain the metric node sequence. The metric node sequence and the marker node sequence are completely identical in length and correspond one-to-one. Each metric node contains the fused pose data and a timestamp for that moment.

[0126] It should be noted that by extracting the fused pose corresponding to the visual marker moment from the first motion trajectory, a standard reference is provided for subsequent deviation calculation; ensuring the comparability between the marker node and the measurement node, the measurement node sequence retains the high-precision characteristics of the first motion trajectory, and subsequent deviation calculation can utilize the local accuracy of the fusion of inertial data and distance data.

[0127] Specifically, for each marker node in the marker node sequence, its timestamp is obtained and denoted as t_m. The two metric nodes with timestamps closest to t_m are found in the metric node sequence. The entire metric node sequence is traversed, and the metric node with a timestamp less than t_m and closest to t_m is selected as the preceding metric node; the metric node with a timestamp greater than t_m and closest to t_m is selected as the following metric node. These two nodes correspond to the two most recent fused pose output times before and after the marker detection time, respectively.

[0128] After obtaining the preceding and following measurement nodes, two first connecting edges are established: one connecting the marker node to the preceding measurement node, and the other connecting the marker node to the following measurement node. The constraint information associated with each first connecting edge is the relative pose transformation between the two nodes, i.e., the rotation and translation required to transform the pose from the source node to the target node. For the edge from the marker node to the preceding measurement node, the constraint indicates the relative position of the marker node from the perspective of the preceding measurement node; the same applies to the edge from the marker node to the following measurement node. These constraints will be used as error terms in subsequent graph optimization calculations. The confidence weight of the edge can be set according to the deviation value of the marker node itself: the smaller the positional deviation value of the marker node, the more reliable the marker is, and the higher the weight of its corresponding edge can be set; conversely, for marker nodes with large deviation values ​​but not yet reaching the failure threshold, the weight of their corresponding edges can be appropriately reduced. The failure threshold can be set to a fixed 0.1 meters according to the system accuracy requirements.

[0129] It should be noted that by establishing bidirectional connections between each marker node and the preceding and following measurement nodes, a deep fusion of visual observation information and motion trajectory is achieved. The bidirectional connection allows the marker node to be constrained by motion trends from both directions simultaneously. Compared to connecting only one measurement node, this can more accurately locate the reasonable position of the marker in the motion trajectory. By dynamically adjusting the weight of the connecting edges based on the deviation value of the marker node itself, the robustness and accuracy of the localization process are improved.

[0130] Specifically, the sequence of metric nodes is traversed. For each pair of temporally adjacent metric nodes, the pose data of the preceding and following nodes are extracted, and the relative pose transformation is calculated to reflect the robot's actual motion increment within that time, including rotational and displacement changes. The calculation of this relative transformation is directly derived from the poses of the two nodes. The calculated relative pose transformation is used as the constraint information for the second connecting edge, connecting the corresponding nodes.

[0131] Preferably, the confidence weights of the second connecting edges can be set based on the uncertainties in the fusion process of inertial and distance data. In the extended Kalman filter, each fusion update outputs the covariance matrix of the current state estimate, which reflects the uncertainty of the state estimate. The covariance of the relative pose transformation between adjacent time points can be derived from this covariance matrix and used as the weight matrix of the second connecting edges. Typically, relative motion estimates over short periods have higher confidence, and the weights of the second connecting edges are set higher. The above process is repeated for all adjacent metric node pairs to construct the second connecting edge network.

[0132] It should be noted that by constructing a second connecting edge network, the entire pose graph can maintain structural stability and continuity even when visual labels are sparse or ineffective. The edge weights are dynamically set based on the filter's covariance matrix, reflecting the confidence level of each motion increment and enabling reasonable allocation of confidence levels for motion at different time intervals. The dense network of second connecting edges ensures the smoothness of the robot trajectory and the rationality of the motion, avoiding trajectory abrupt changes caused by visual label errors and improving the accuracy of the localization results.

[0133] Specifically, an empty graph data structure is created, using an adjacency list or adjacency matrix to store the relationships between nodes and edges. All nodes in the labeled node sequence are added to the graph, each assigned a unique identifier and its attributes stored, including node type, labeled identity, visual positioning pose, timestamp, and positional deviation. All nodes in the metric node sequence are also added to the graph, similarly assigned unique identifiers, and their node type, fused pose, and timestamp stored.

[0134] After adding all nodes, iterate through all temporally adjacent metric node pairs and add each calculated second connection edge to the graph. Each edge records the identifiers of the two connected nodes, constraint information, and the corresponding weight matrix. For each marker node, iterate through the preceding and following metric nodes and add two first connection edges. Each edge records the identifiers of the marker and metric nodes, their relative pose transformation, and the weight set based on the marker node's position deviation. After adding all nodes and edges, perform a consistency check to ensure that both nodes connected by each edge exist, thus obtaining the first pose graph.

[0135] It should be noted that by integrating nodes and edges, a first pose graph is constructed, which fuses visual observation information with motion estimation information to obtain a pose graph that reflects absolute position while maintaining the continuity of relative motion. The edges in the graph store constraint information, and the uncertainty of each constraint is quantified through a weight matrix, providing accurate data support for localization analysis.

[0136] Furthermore, the deviation between the marked node and the corresponding metric node is calculated to identify the position failure nodes in the first pose graph, resulting in a set of failure nodes, including:

[0137] S701. Calculate the deviation between the marker node and the corresponding connection metric node to obtain the first deviation value;

[0138] S702. Analyze the positional deviation of the marked nodes by combining the historical positional deviation sequence to obtain the second deviation value;

[0139] S703. Combining the first deviation value and the second deviation value, identify the position failure nodes in the first pose graph to obtain the set of failure nodes.

[0140] In this embodiment, in the constructed first pose graph, each marker node is connected to two preceding and following metric nodes via a first connecting edge. These two edges represent the constraint relationship between the marker node and the robot's motion trajectory. For each marker node, the pose information of the two preceding and following metric nodes connected to it is obtained, denoted as the preceding metric node pose and the following metric node pose, respectively. These two metric nodes are themselves connected by a second connecting edge, which stores the relative pose transformation from the former to the latter. This transformation represents the actual motion increment of the robot within this short time interval. Based on the motion increment, a prediction model is constructed: starting from the preceding metric node pose, along the motion direction and distance indicated by the second connecting edge, a theoretical following metric node pose is predicted; starting from the following metric node pose, applying the motion increment in reverse, a theoretical preceding metric node pose can be predicted. The reasonable position of the marker node should be located on the motion path between these two metric nodes.

[0141] Specifically, the relative transformations between the poses of the marked node and the preceding metric node, and between the poses of the marked node and the following metric node, are calculated separately. These two relative transformations are then compared with the relative transformations stored in the second connecting edge. A portion of the relative transformation from the preceding metric node to the marked node should be consistent with a portion of the relative transformation from the preceding metric node to the following metric node; similarly, a portion of the relative transformation from the marked node to the following metric node should be consistent with a portion of the reverse transformation from the following metric node to the preceding metric node. By comparing the differences in the position components of these relative transformations, two deviation components are obtained. These two deviation components are combined, and their average Euclidean distance is taken to obtain the first deviation value of the marked node.

[0142] It should be noted that by calculating the deviation between the marker node and the adjacent metric node, the relationship between the marker node and the two preceding and following metric nodes is comprehensively considered. This method is more effective in reflecting the reasonable position of the marker in the local motion trajectory than comparing it with only a single node. It can quickly capture sudden anomalies in marker observation and provide accurate data support for real-time failure detection.

[0143] Specifically, from the positional deviation sequence, all historical deviation records with the same identifier as the currently labeled node are extracted. These records are arranged in timestamp order, forming the deviation time series of that labeled individual. A sliding window mechanism is used, focusing only on data within a recent period, such as the last ten detections or all detections within the last thirty minutes, to avoid undue influence from outdated data on the current judgment. The choice of window size requires a trade-off between response speed and stability: a window that is too small is easily affected by short-term fluctuations, while a window that is too large is not sensitive enough to recent changes.

[0144] After obtaining the historical deviation sequence within the window, statistical characteristics are calculated, including but not limited to the sequence mean, reflecting the average deviation level of the label; the sequence standard deviation, reflecting the degree of deviation fluctuation; the first difference mean or median of the sequence, reflecting the trend of deviation; and the sequence maximum value, reflecting the worst historical performance. Based on these statistical characteristics, a second deviation value is constructed. For example, the second deviation value is defined as the degree of deviation between the most recent deviation before the current time and the historical mean, i.e., the standardized deviation is obtained by subtracting the historical mean from the current deviation and dividing by the historical standard deviation.

[0145] It should be noted that by analyzing the statistical characteristics of the historical deviation sequence, a second deviation value reflecting the long-term health status of each marker was obtained. This value can identify progressive failure modes and effectively distinguish between accidental instantaneous noise and continuous systematic deviations, avoiding misjudgment of marker failure due to a single large deviation. The calculation of the second deviation value provides a historical perspective for subsequent failure identification, complementing the first deviation value and making the judgment results more comprehensive and accurate.

[0146] Specifically, each marked node in the first pose graph is traversed. For the current marked node, a first deviation value and a second deviation value are obtained. A first deviation threshold and a second deviation threshold are set. The first deviation threshold is set according to the system's positioning accuracy requirements, and can be set to 0.08 meters. The second deviation threshold can be set to the historical average plus three standard deviations. If both the first deviation value and the second deviation value exceed the first deviation threshold, the marked node is determined to be a location failure node. Marked nodes determined to be failures are added to the failure node set.

[0147] It should be noted that by fusing the first and second deviation values, the failed marker nodes can be accurately identified. The fusion of the two indicators can reduce the false judgment rate and avoid the wrong removal of normal markers due to single noise or brief interference, thereby improving the stability of the system. The establishment of the failed node set provides accurate data support for subsequent graph reconstruction, improving the accuracy and robustness of the positioning results.

[0148] Furthermore, based on the set of failed nodes, the failed nodes and their corresponding connecting edges are removed from the first pose graph. The positions of the failed nodes corresponding to the first movement trajectory are analyzed, and virtual nodes are added to obtain the second pose graph, including:

[0149] S801. According to the set of failed nodes, remove the failed nodes and their corresponding connecting edges from the first pose graph to obtain the updated pose graph;

[0150] S802. Based on the updated pose graph, analyze the position of the failed node corresponding to the first movement trajectory, add virtual nodes, and obtain the second pose graph.

[0151] In this embodiment, the set of failed nodes is traversed. For each failed node identifier, the corresponding node object is searched in the node list of the first pose graph. After finding the target node, all edges connected to that node are deleted, including the first connecting edge connecting the marked node to the preceding and following metric nodes. The edge list of the graph structure is traversed to find all edges whose starting or target node is the failed node, and these edges are removed from the edge list. After deleting the edges, the failed node is removed from the node list. The above process is repeated for each node in the set of failed nodes until all failed nodes and their associated edges are completely cleared, resulting in an updated pose graph.

[0152] It should be noted that by removing failed nodes and their associated edges, the impact of erroneous observations on the localization results can be reduced, the propagation path of erroneous constraints can be blocked, the failure markers can be prevented from affecting the localization results, and the updated pose graph retains reliable metric nodes and marker nodes, thereby improving the accuracy and efficiency of subsequent localization analysis.

[0153] Specifically, based on the updated pose graph, the positions of the failed nodes corresponding to the first movement trajectory are analyzed, and virtual nodes are added to obtain the second pose graph. Adding virtual nodes can compensate for the degradation of failure markers, filling the information gaps caused by the removal of failed nodes and preventing trajectory breaks due to missing nodes. The construction of virtual nodes combines local historical motion trends and global adjacency constraints, ensuring that the pose conforms to the robot's actual motion trajectory. By compensating for the nodes, the continuity and smoothness of localization can be maintained even when visual markers fail, avoiding localization jumps or interruptions caused by missing markers, thus improving the robustness and environmental adaptability of the localization system.

[0154] Furthermore, based on the updated pose graph, the positions of the failed nodes corresponding to the first movement trajectory are analyzed, virtual nodes are added, and a second pose graph is obtained, including:

[0155] S901. Based on the updated pose graph, analyze the position of the failed node corresponding to the first moving trajectory, extract the trajectory segment of the failed node in the first moving trajectory, and obtain the trajectory segment set.

[0156] S902. Integrate each trajectory segment in the trajectory segment set to obtain the first relative pose, and calculate the pose difference between adjacent metric nodes connected to the failed node to obtain the second relative pose.

[0157] S903. Combining the first relative pose and the second relative pose, construct the virtual pose of the virtual node and add it to the corresponding failed node position to obtain the second pose diagram.

[0158] In this embodiment, the set of failed nodes is traversed. For each failed node in the set, timestamp information is obtained. The timestamp is the moment when the visual marker corresponding to the failed node was detected. The search is performed in the first movement trajectory based on the timestamp. A time window is determined with the timestamp of the failed node as the center. The choice of window size needs to consider the robot's movement speed and the density of trajectory data. For example, when the robot's typical movement speed is 0.5 meters per second, a time window of 1 second before and after can be selected, that is, a trajectory segment of 2 seconds in total; or a spatial range of 0.5 meters before and after can be selected.

[0159] After determining the time window, all fused poses within that time window are extracted from the first movement trajectory to form a continuous trajectory segment. This trajectory segment is an ordered list, where each element contains a timestamp and its corresponding fused pose. The above process is repeated for each failed node in the set of failed nodes to obtain multiple trajectory segments, which are then integrated to form a trajectory segment set.

[0160] It should be noted that by extracting local trajectory fragments near the failure node, historical motion information can be located and collected, providing an accurate data foundation for the subsequent construction of virtual nodes. By designing a time window centered on the failure node's timestamp, the extracted trajectory fragments are ensured to be closely correlated with the failure node in time, accurately reflecting the robot's actual motion state when passing through that location, thus improving the accuracy of the virtual nodes and the accuracy of the robot's localization results.

[0161] Specifically, for each trajectory segment in the trajectory segment set, the relative transformation between adjacent poses is calculated sequentially, and these relative transformations are accumulated. The accumulation process employs a composite operation of pose transformations, where each subsequent relative transformation acts on the previous accumulated result to obtain the composite transformation of the trajectory segment, which serves as the first relative pose, describing the robot's overall motion direction and distance within this time window. The update pose graph identifies two metric nodes adjacent to the currently failed node; these two metric nodes are the preceding and following metric nodes connected to the failed node via a first connecting edge. The pose data of these two metric nodes are acquired, and the relative pose transformation is calculated—that is, the rotation and translation of the latter node's pose relative to the former node's pose—to obtain the second relative pose. The second relative pose reflects the overall motion change the robot should have when traversing this region from the perspective of adjacent reliable nodes.

[0162] It should be noted that by calculating the relative pose information from two sources, complementary constraints are provided for the subsequent construction of virtual nodes. The first relative pose comes from the integral of the local continuous trajectory, which preserves the details and continuity of the robot's motion and reflects the specific path when actually passing through the area. The second relative pose comes from the constraints of the global adjacent metric nodes, which reflects the motion relationship that the area should have under the overall graph framework and has global consistency. By calculating a pair of relative poses for each failed node, local distortion caused by uniform compensation is avoided, and the accuracy and robustness of the localization results are improved.

[0163] For each failed node, determine the temporal position of the virtual node. Since the failed node originally corresponds to a specific time, the virtual node should be inserted into the updated pose graph at the position corresponding to that time. The time of the virtual node is taken as the original time of the failed node. After determining the temporal position, construct the spatial pose of the virtual node. Denote the pose of the virtual node as P_v, the pose of the preceding metric node as P_before, and the pose of the following metric node as P_after. The relative transformation from P_before to P_v should be consistent with the part from the start of the trajectory segment to the time of failure; the relative transformation from P_v to P_after should be consistent with the part from the time of failure to the end of the segment; the overall transformation from P_before to P_after should be equal to the second relative pose. Combining these three conditions, construct a least-squares optimization problem: with P_v as the optimization variable, the objective function includes the deviation between P_v and the position predicted based on the first relative pose, and the deviation between the overall transformation from P_before to P_after and the second relative pose. Solving this optimization problem yields P_v, which minimizes the total error, representing the virtual pose of the virtual node.

[0164] After constructing the pose of the virtual node, it is added as a new node to the updated pose graph, and two new connecting edges are established: one connecting the previous metric node and the virtual node, and the other connecting the virtual node and the next metric node. The constraint information of these two edges is the corresponding relative pose transformation, and their weights can be set according to the confidence level constructed by the virtual node. The above process is repeated for each failed node in the set of failed nodes. All gaps in the updated pose graph caused by the removal of failed nodes are filled by virtual nodes, resulting in the second pose graph.

[0165] It should be noted that by constructing virtual nodes by fusing local and global information, accurate compensation for failure markers is achieved. Virtual nodes fill the gaps caused by the removal of failure nodes, ensuring that the second pose graph maintains a complete topological structure. The pose of the virtual nodes fuses local motion trends and global adjacency constraints, conforming to the actual motion path of the robot when passing through the area, and maintaining geometric consistency with surrounding nodes. Through optimization, the virtual nodes can find the optimal balance between the two constraints, providing an accurate structural foundation and data support for subsequent high-precision positioning calculations.

[0166] like Figure 3 As shown, a visual positioning system combining image data is used to implement a visual positioning method combining image data, including:

[0167] The data acquisition module acquires motion data during the movement of the mobile robot, including image data, inertial data, and distance data.

[0168] The position analysis module identifies visual markers based on image data and analyzes the visual positioning results. Combining inertial data and distance data, it analyzes and obtains the first movement trajectory, calculates the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory, and obtains the position deviation sequence.

[0169] The failure detection module, based on the position deviation sequence, fuses the visual positioning results and the first movement trajectory to construct the first pose map, identifies the position failure nodes in the first pose map, and obtains the set of failure nodes;

[0170] The robot localization module removes the failed nodes and their corresponding connecting edges from the first pose graph according to the set of failed nodes, analyzes the position of the failed nodes corresponding to the first movement trajectory, adds virtual nodes, and obtains the second pose graph to locate the mobile robot.

[0171] The above description is merely a preferred embodiment of this application. The scope of protection of this application is not limited to the above embodiments. All technical solutions falling within the scope of this application's concept are within the scope of protection of this application. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of this application should also be considered within the scope of protection of this application.

Claims

1. A visual localization method combining image data, characterized in that, include: Acquire motion data during the movement of the mobile robot, including image data, inertial data, and distance data; Visual markers are identified based on image data, and the visual positioning results are analyzed. Combined with inertial data and distance data, the first movement trajectory is obtained. The deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory is calculated to obtain the position deviation sequence. Based on the position deviation sequence, the visual positioning results and the first movement trajectory are fused to construct the first pose map, identify the position failure nodes in the first pose map, and obtain the set of failure nodes. Based on the set of failed nodes, the failed nodes and their corresponding connecting edges are removed from the first pose graph. The positions of the failed nodes corresponding to the first movement trajectory are analyzed, and virtual nodes are added to obtain the second pose graph for locating the mobile robot.

2. The visual positioning method combining image data according to claim 1, characterized in that, The process involves identifying visual markers based on image data, analyzing the visual positioning results, combining inertial data and distance data to obtain a first movement trajectory, calculating the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory, and obtaining a position deviation sequence, including: Visual markers are identified based on image data, and the pixel positions corresponding to the visual markers are extracted to analyze the visual positioning results. Combined with inertial data and distance data, the first movement trajectory is obtained. The deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory is calculated to obtain the position deviation sequence.

3. The visual positioning method combining image data according to claim 2, characterized in that, The step of identifying visual markers based on image data, extracting the pixel positions corresponding to the visual markers, analyzing the visual positioning results, and combining inertial data and distance data to obtain the first movement trajectory includes: Visual markers are identified based on image data, and their pixel positions in the image are extracted to obtain a set of pixel positions. Based on the set of pixel positions, the rotation matrix and translation vector between the image coordinates and the robot spatial coordinates are calculated, the visual positioning pose is analyzed, and the visual positioning result is obtained. The attitude change is obtained by integrating the angular velocity in the inertial data, and the displacement change is obtained by integrating the acceleration in the inertial data. The inertial pose is calculated by combining the attitude change and the displacement change. The inertial pose is corrected based on the distance data to obtain the fused pose; Arrange the fused poses in chronological order to obtain the first movement trajectory.

4. The visual positioning method combining image data according to claim 3, characterized in that, The step of calculating the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory to obtain a position deviation sequence includes: Transform the visual positioning results and the first movement trajectory to the same coordinate system; The positional deviation sequence is obtained by calculating the difference between the visual positioning result of each visual marker in the same coordinate system and the corresponding position in the first movement trajectory.

5. The visual positioning method combining image data according to claim 4, characterized in that, The method involves constructing a first pose map based on the position deviation sequence, fusing visual positioning results and the first movement trajectory, identifying position failure nodes in the first pose map, and obtaining a set of failure nodes, including: Based on the position deviation sequence, the visual positioning result and the first movement trajectory are fused. The visual positioning pose corresponding to the visual positioning result is used as the marker node, and the fused pose in the first movement trajectory is used as the metric node. A connection edge is established between the marker node and the metric node to construct the first pose graph. Calculate the deviation between the marked node and the corresponding metric node, identify the position failure nodes in the first pose graph, and obtain the set of failure nodes.

6. The visual positioning method combining image data according to claim 5, characterized in that, The step of fusing visual positioning results and a first movement trajectory based on the position deviation sequence, using the corresponding visual positioning pose in the visual positioning results as a marker node and the fused pose in the first movement trajectory as a metric node, and establishing connection edges between the marker nodes and the metric nodes to construct the first pose graph includes: Based on the multiple visual positioning poses and position deviation sequences corresponding to each visual marker, the visual positioning pose with the smallest position deviation is selected as the marker node, thus obtaining the marker node sequence. The fused pose at the time corresponding to the visual marker in the first movement trajectory is used as the metric node to obtain the metric node sequence; For each labeled node in the labeled node sequence, select the adjacent preceding and following metric nodes from the metric node sequence and establish the first connecting edge; For adjacent metric nodes in the metric node sequence, a second connection edge is established by calculating the relative pose transformation; By combining the marker node, the metric node, the first connecting edge, and the second connecting edge, the first pose graph is constructed.

7. The visual positioning method combining image data according to claim 6, characterized in that, The deviation between the calculated marker node and the corresponding metric node is used to identify the position failure nodes in the first pose graph, resulting in a set of failure nodes, including: Calculate the deviation between the marked node and the corresponding connection metric node to obtain the first deviation value; By combining historical position deviation sequence analysis with the position deviation of the marked nodes, the second deviation value is obtained; By combining the first deviation value and the second deviation value, the position failure nodes in the first pose graph are identified, and the set of failure nodes is obtained.

8. The visual positioning method combining image data according to claim 1, characterized in that, The process involves removing failed nodes and their corresponding connecting edges from the first pose graph according to the set of failed nodes, analyzing the positions of the failed nodes corresponding to the first movement trajectory, adding virtual nodes, and obtaining the second pose graph, including: Based on the set of failed nodes, remove the failed nodes and their corresponding connecting edges from the first pose graph to obtain the updated pose graph; Based on the updated pose graph, the positions of the failed nodes corresponding to the first movement trajectory are analyzed, virtual nodes are added, and the second pose graph is obtained.

9. The visual positioning method combining image data according to claim 8, characterized in that, The process of analyzing the positions of failed nodes corresponding to the first movement trajectory based on the updated pose graph, adding virtual nodes, and obtaining the second pose graph includes: Based on the updated pose graph, the position of the failure node corresponding to the first moving trajectory is analyzed, and the trajectory fragments of the failure node in the first moving trajectory are extracted to obtain a set of trajectory fragments. The first relative pose is obtained by integrating each trajectory segment in the trajectory segment set, and the second relative pose is obtained by calculating the pose difference between adjacent metric nodes connected to the failed node. By combining the first relative pose and the second relative pose, a virtual pose of the virtual node is constructed and added to the corresponding failed node position to obtain the second pose graph.

10. A visual positioning system combining image data, characterized in that, A method for implementing visual localization combining image data as described in any one of claims 1 to 9, comprising: The data acquisition module acquires motion data during the movement of the mobile robot, including image data, inertial data, and distance data. The position analysis module identifies visual markers based on image data and analyzes the visual positioning results. Combining inertial data and distance data, it analyzes and obtains the first movement trajectory, calculates the deviation between the visual positioning result of each visual marker and the corresponding position in the first movement trajectory, and obtains the position deviation sequence. The failure detection module, based on the position deviation sequence, fuses the visual positioning results and the first movement trajectory to construct the first pose map, identifies the position failure nodes in the first pose map, and obtains the set of failure nodes; The robot localization module removes the failed nodes and their corresponding connecting edges from the first pose graph according to the set of failed nodes, analyzes the position of the failed nodes corresponding to the first movement trajectory, adds virtual nodes, and obtains the second pose graph to locate the mobile robot.

Images