Autonomous positioning and deviation rectification method and system for mobile robot based on visual odometry

Abstract

The application relates to the technical field of robot positioning and deviation rectification, and discloses a mobile robot autonomous positioning and deviation rectification method and system based on a visual odometer, which comprises the following steps: analyzing road edges and constructing road constraints according to motion data and environment data collected by a mobile robot in real time; performing enhanced processing on visual odometer information, coupling the enhanced visual odometer information with the motion data and the environment data, analyzing the spatial constraints and the state of the robot relative to fixed objects, and obtaining first pose information; combining the first pose information with a preset environment map, predicting the pose change trend of the robot, and obtaining second pose information; analyzing the trajectory deviation according to the second pose information and a preset mobile task, generating corresponding deviation rectification instructions, controlling the real-time movement of the mobile robot, and performing autonomous positioning and deviation rectification by the mobile robot; and the application can effectively inhibit the cumulative error of the visual odometer and improve the positioning and deviation rectification accuracy.

Description

Technical Field

[0001] This application relates to the field of robot positioning and correction technology, and more specifically to a method and system for autonomous positioning and correction of mobile robots based on visual odometry. Background Technology

[0002] Currently, autonomous localization and navigation of mobile robots are fundamental for intelligent operations. Autonomous localization of mobile robots mainly relies on visual odometry, inertial navigation, and multi-sensor fusion technology. Visual odometry estimates motion through image sequences but suffers from cumulative errors; inertial navigation offers strong independence, but its errors diverge over time; multi-sensor fusion can improve robustness, but in structured environments such as factory corridors and park paths, it fails to fully utilize road geometry to correct positioning drift, leading to a decline in long-term operational accuracy.

[0003] Existing technologies have the following problems: single visual odometry is prone to feature tracking failure in environments with sparse textures, changing lighting, or interference from dynamic objects, and cannot avoid pose estimation drift; road detection is only used for obstacle avoidance or coarse path following, and cannot be combined with road geometric parameters for pose estimation optimization; when there are temporary occlusions, blurred road boundaries, or interaction with other moving objects in the environment, the stability of the localization system decreases, and sudden errors are easily generated; in order to solve at least one of the above problems, this application proposes a method and system for autonomous localization and correction of mobile robots based on visual odometry. Summary of the Invention

[0004] To address the shortcomings of existing technologies, the purpose of this application is to provide a method and system for autonomous localization and correction of mobile robots based on visual odometry, which can effectively solve the problems in the background technology. The specific technical solution of this application is as follows: A visual odometry-based autonomous localization and correction method for mobile robots includes: Based on the motion and environmental data collected in real time by sensors on the mobile robot, road parameters are analyzed to identify road edges and construct road constraints. The visual odometry information is enhanced according to road constraints. The enhanced visual odometry information, motion data and environmental data are coupled and analyzed to analyze the robot’s spatial constraints and state relative to the fixed object, and obtain the first pose information. By combining the first pose information and the preset environment map, the robot's pose change trend is predicted to obtain the second pose information; Based on the second pose information and the preset movement task, the trajectory deviation is analyzed, corresponding correction instructions are generated, and the real-time movement of the mobile robot is controlled to enable the mobile robot to perform autonomous positioning and correction.

[0005] Specifically, the step of analyzing road parameters to identify road edges and constructing road constraints based on motion data and environmental data collected in real time by sensors on the mobile robot includes: Based on the motion data and environmental data collected in real time by the sensors on the mobile robot, the road parameters are analyzed to identify the road edge, and the straight line segments of the road edge are extracted to obtain a set of edge line segments; Analyze the edge coordinates of the edge segments, filter out the edge segments whose edge coordinates are less than a preset distance threshold from the robot coordinates, and construct road constraints.

[0006] Specifically, the analysis of edge coordinates of edge segments, filtering out edge segments in the edge segment set whose edge coordinates are less than a preset distance threshold from the robot coordinates, and constructing road constraints include: Analyze the edge coordinates of the edge segments, and filter out the edge segments in the edge segment set whose midpoint coordinates are less than the robot coordinates by a preset distance threshold, thus obtaining the first segment set. Based on the robot's current position, the movement curvature of the corresponding position point is analyzed. Edge line segments whose absolute value of the difference between the line segment curvature and the movement curvature is less than a preset curvature threshold are selected from the first line segment set to obtain the second line segment set. In the second set of line segments, the edge line segments on both sides are fitted and connected to obtain the first boundary line and the second boundary line; By combining the analysis of the first and second boundary lines to determine the road centerline, and filtering out the movement area based on the robot's real-time position, the robot's movement direction constraints and movement distance constraints are set to obtain the road constraints.

[0007] Specifically, the visual odometry information is enhanced according to road constraints, and the enhanced visual odometry information is coupled and analyzed with motion data and environmental data to analyze the robot's spatial constraints and state relative to the fixed object, thereby obtaining the first pose information, including: The visual odometer information is enhanced according to road constraints. Feature points are extracted from the visual odometer information and mapped onto the road centerline to obtain the enhanced visual odometer information. The enhanced visual odometry information, motion data, and environmental data are coupled and analyzed to analyze the robot's spatial constraints and state relative to the fixed object, thus obtaining the first pose information.

[0008] Specifically, the enhancement processing of visual odometer information according to road constraints, extracting feature points from the visual odometer information and mapping them onto the road centerline to obtain enhanced visual odometer information, includes: Feature points are extracted from visual odometry information to obtain the first set of feature points within road constraints; For each feature point in the first feature point set, calculate the vertical distance between the projection point onto the road and the road centerline, set the corresponding weight according to the vertical distance, calculate the weighted feature points, and obtain the second feature point set; Select the first feature point closest to the first boundary line and the second feature point closest to the second boundary line from the second feature point set; The enhancement factor is calculated by combining the distance between the first feature point and the second feature point; Each feature point in the second feature point set is enhanced according to the enhancement factor, and the enhanced feature points are integrated and optimized to obtain enhanced visual odometry information.

[0009] Specifically, the enhanced visual odometry information, motion data, and environmental data are coupled and analyzed to analyze the robot's spatial constraints and state relative to the stationary object, obtaining the first pose information, including: According to the preset time window, the enhanced visual odometry information, motion data and environmental data are coupled and analyzed to obtain the first fusion result. The coupling analysis process includes: time alignment of the enhanced visual odometry information, motion data and environmental data within the sliding time window, calculation of motion increment by data integration, calculation of the data weight corresponding to the minimum visual projection error, and fusion according to the data weight. Based on the first fusion result and the distance between the robot and the fixed object, the spatial constraints and state of the robot relative to the fixed object are analyzed to obtain the pose sequence; By combining the pose sequence and road constraints, the deviation between the robot and the road is analyzed, and the pose sequence is corrected to obtain the first pose information.

[0010] Specifically, the process of combining the first pose information and a preset environmental map to predict the robot's pose change trend and obtain the second pose information includes: Combining the first pose information and the preset environment map, starting from the robot's current position, the curvature and slope information of the corresponding road are analyzed to select the first desired trajectory; Based on the robot's current speed and power parameters, predict the robot's pose change trend to obtain the second desired trajectory; Compare the deviation between the first expected trajectory and the second expected trajectory, and calculate the trajectory offset; By combining the trajectory offset, the first desired trajectory, and the second desired trajectory, offset correction is performed on each trajectory change point of the robot to obtain the second pose information.

[0011] Specifically, the step of analyzing trajectory deviation based on the second pose information and the preset movement task, generating corresponding correction commands, and controlling the real-time movement of the mobile robot to perform autonomous positioning and correction includes: Based on the second pose information and the preset movement task, the trajectory offset is analyzed to obtain the trajectory deviation; The control time domain and control parameters are calculated based on the trajectory deviation, and corresponding correction commands are generated to control the real-time movement of the mobile robot.

[0012] Specifically, the step of calculating the control time domain and control parameters according to the trajectory deviation, generating corresponding correction commands, and controlling the real-time movement of the mobile robot includes: Based on the trajectory deviation, the deviation point with the largest trajectory deviation is selected, and a control time domain is constructed with the deviation point as the center. The control parameters corresponding to each position in the control time domain are calculated. By combining the control time domain, control parameters, and the robot's real-time status, corresponding correction commands are generated to control the real-time movement of the mobile robot.

[0013] A visual odometry-based autonomous localization and correction system for mobile robots is used to implement the aforementioned visual odometry-based autonomous localization and correction method for mobile robots, including: The road analysis module analyzes road parameters, identifies road edges, and constructs road constraints based on real-time motion and environmental data collected by sensors on the mobile robot. The pose analysis module enhances the visual odometry information according to road constraints, and couples the enhanced visual odometry information with motion data and environmental data to analyze the robot’s spatial constraints and state relative to the fixed object, and obtains the first pose information. The pose prediction module combines the first pose information with a preset environmental map to predict the robot's pose change trend and obtain the second pose information. The positioning and correction module analyzes the trajectory deviation based on the second pose information and the preset movement task, generates corresponding correction commands, and controls the real-time movement of the mobile robot to perform autonomous positioning and correction.

[0014] The beneficial effects of this application are as follows: By identifying road edges and constructing road constraints related to the robot's current position and posture, visual information is projected onto the road and coupled with environmental data to enhance the visual information and analyze the robot's spatial position and real-time state. Based on the road constraints, deviation analysis and correction of the pose are performed, controlling the robot's real-time movement and enabling autonomous localization and correction. By constructing road constraints and enhancing visual information, the cumulative error of visual odometry can be effectively suppressed, maintaining high accuracy during long-distance operation. Real-time deviation correction of the robot's pose improves the environmental adaptability of the localization process and enhances the accuracy of localization and correction. Attached Figure Description

[0015] Figure 1 This is a flowchart illustrating the autonomous localization and correction method for a mobile robot based on visual odometry in Embodiment 1 of this application. Figure 2 This is a schematic diagram illustrating the division of the left-side candidate set and the right-side candidate set of line segments in Embodiment 1 of this application; Figure 3 This is a flowchart illustrating the second pose information calculation process in Embodiment 1 of this application; Figure 4 This is a schematic diagram of the structure of the mobile robot autonomous positioning and correction system based on visual odometry in Embodiment 1 of this application. Detailed Implementation

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

[0017] 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.

[0018] 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.

[0019] Example 1: refer to Figure 1 The image shows a specific implementation of the visual odometry-based autonomous localization and correction method for mobile robots according to this application, including: S101. Based on the motion data and environmental data collected in real time by the sensors on the mobile robot, analyze the road parameters to identify the road edges and construct road constraints. S102. Enhance the visual odometry information according to road constraints, and couple the enhanced visual odometry information with motion data and environmental data to analyze the spatial constraints and state of the robot relative to the fixed object, and obtain the first pose information. S103. Combining the first pose information and the preset environment map, predict the robot's pose change trend to obtain the second pose information. S104. Based on the second pose information and the preset movement task, analyze the trajectory deviation, generate corresponding correction instructions, and control the real-time movement of the mobile robot to perform autonomous positioning and correction of the mobile robot.

[0020] In this embodiment, raw data is acquired in real time by sensors mounted on the robot, including but not limited to monocular / binocular cameras, LiDAR, and inertial measurement units (IMUs). Motion data includes trajectory estimation information from wheel encoders and the IMU, while environmental data includes camera images or LiDAR point clouds. Image processing is used to extract a set of line segments reflecting the road boundary from the environmental data. The coordinates of the midpoints of each line segment are analyzed and aligned. The coordinates of the midpoints of the line segments are compared with the current robot coordinates calculated based on the motion data. Line segments whose midpoint coordinates are less than a preset distance threshold from the robot are retained, resulting in a set of road edges and the construction of road constraints.

[0021] It should be noted that by dynamically filtering road edges related to the robot's current position, effective road constraints can be constructed; by presetting a distance threshold to filter out the robot's nearby area, the real-time nature of constraint construction can be ensured, and the targeting and efficiency of environmental information utilization can be improved; the quality of the filtered road edge line segment set is higher, reducing noise introduced by inaccurate detection of distant edges or false detection of non-road edges, which can improve the accuracy of road constraints.

[0022] Specifically, from the pose transformation information and map points output by the visual odometry, a set of three-dimensional feature points located within the current road constraint area is extracted as the first feature point set. For each feature point, the point vertically projected onto the road plane is calculated, and the vertical distance from the projected point to the road centerline is calculated. Weights are assigned according to the vertical distance, and the coordinates of the feature points are weighted according to the weights to generate a weighted second feature point set. From the second feature point set, the two feature points closest to the left and right road boundaries are identified, the Euclidean distance between them is calculated, and an enhancement factor is calculated. The weights of all feature points in the second feature point set are enhanced using the enhancement factor to obtain the enhanced visual odometry information.

[0023] Preferably, the enhanced visual odometry information is timestamped with the angular velocity and acceleration data of the IMU and the odometer data of the wheel encoder within a sliding time window. The enhanced visual odometry information is coupled with the weighted minimization of visual reprojection error, IMU pre-integration error and wheel speedometer error through extended Kalman filtering. After fusion, a preliminary robot motion trajectory that integrates multi-sensor information is obtained as a pose sequence. The lateral deviation and heading deviation of each point on the trajectory relative to the road centerline are calculated according to road constraints. The deviations are smoothed and corrected using Kalman filtering to obtain the first pose information after strong road geometric constraint correction.

[0024] It should be noted that by combining road constraint analysis with visual information and enhancing it, the problem of visual feature mismatch caused by repeated environmental textures, dynamic object occlusion, or sudden changes in lighting can be effectively suppressed, thereby improving the reliability and accuracy of visual odometry. The enhancement factor is calculated based on road constraints, and the weight of the data fusion process is dynamically adjusted. When the road boundary is clear and the visual information is reliable, more reliance is placed on visual information. When at intersections or in areas with blurred boundaries, the system smoothly transitions to relying more on inertial and wheel odometer information, thereby improving the continuity and robustness of the positioning system.

[0025] Specifically, starting from the current position and combining it with a pre-set environmental map, a reference trajectory of the road centerline within a certain distance ahead is extracted as the first expected trajectory based on road geometry. Based on the robot's current speed and acceleration, a second expected trajectory is predicted within the same future time period, generated solely by the inertial extrapolation and dynamic response of the current motion state. At multiple discrete future time points, the lateral position deviation and heading deviation of these two trajectories are compared to calculate the trajectory offset sequence. According to the calculated offset, the robot's predicted trajectory is compensated. For example, if it is predicted that the robot will deviate from the road centerline by 0.2 meters in 2 seconds, when generating the final second pose information, the position of the predicted point is pre-adjusted inward by 0.1 meters towards the road centerline. This process is repeated for multiple points on the predicted trajectory to obtain a pre-corrected second pose information sequence that considers dynamic characteristics and actively conforms to the road geometry.

[0026] It should be noted that, compared to traditional state estimation which only outputs the current or historical state, the second pose message sequence output by this method is a prediction of the robot's motion path in the near future. Moreover, this prediction has been actively aligned with the road geometry, avoiding the problem of inconsistency with the road curvature trend caused by pure dynamic prediction. By discovering and compensating for potential offset trends during the prediction stage, the trajectory can be optimized, reducing the adjustment pressure on the controller and improving the smoothness and energy efficiency of the overall system. When facing curves and slopes, the prediction combined with slope and curvature map information can more accurately predict the robot's posture changes, enhancing the system's adaptability to complex road shapes.

[0027] Specifically, the second pose information sequence is compared with the trajectory required by the task to calculate the corresponding trajectory deviation, including lateral deviation and heading deviation. The distribution of these deviations is analyzed to select the point with the largest deviation in the current and recent predictions. The control time domain is determined based on the time position of this point, for example, including the time period from the current point to 0.5 seconds later. At each discrete time step in the control time domain, the control parameters are calculated by the model predictive controller based on the prediction deviation at that moment, the robot's current actual speed, steering angle, and other states. The corresponding correction commands are sent to the robot's underlying drive system to control the motors to execute.

[0028] It should be noted that using the second pose information for positioning and correction can improve the accuracy and efficiency of the correction action, avoiding the control oscillation or correction lag problems caused by high noise and high latency of pose information in traditional methods. By combining model predictive control with the robot's dynamic conditions to calculate the correction command, it is ensured that the generated command is physically executable, preventing robot instability caused by overly aggressive correction commands. This can improve the overall stability and safety of the system, ensuring that the mobile robot can stably, accurately and intelligently complete autonomous navigation and positioning correction tasks on complex structured roads.

[0029] This application constructs road constraints related to the robot's current position and posture by identifying road edges, projects visual information onto the road and couples it with environmental data to enhance the visual information, and analyzes the robot's spatial position and real-time state. Based on the road constraints, it analyzes and corrects pose deviations, controls the robot's real-time movement, and performs autonomous localization and correction. By constructing road constraints and enhancing visual information, the cumulative error of visual odometry can be effectively suppressed, maintaining high accuracy during long-distance operation. Real-time deviation correction of the robot's pose improves the environmental adaptability of the localization process and enhances the accuracy of localization and correction.

[0030] Furthermore, based on the motion data and environmental data collected in real time by sensors on the mobile robot, road parameters are analyzed to identify road edges and construct road constraints, including: S201. Based on the motion data and environmental data collected in real time by the sensors on the mobile robot, analyze the road parameters to identify the road edge, extract the straight line segments of the road edge, and obtain the set of edge line segments. S202. Analyze the edge coordinates of the edge segments, filter out the edge segments in the edge segment set whose edge coordinates are less than the robot coordinates by a preset distance threshold, and construct road constraints.

[0031] In this embodiment, real-time environmental and motion data are acquired. The environmental data includes a sequence of two-dimensional images acquired by a binocular camera and three-dimensional point cloud data acquired by a lidar. The motion data includes the robot's instantaneous displacement and attitude changes acquired by a wheel encoder and an inertial measurement unit. For the image data, the images are preprocessed, including converting them to grayscale and applying Gaussian filtering to smooth noise. The Canny operator in the edge detection operator is used to identify intensity change boundaries in the image, resulting in a binarized edge image. All line segments that conform to the equation of a straight line are detected from the edge image through Hough transform. These line segments are represented by the pixel coordinates of the start and end points in the image coordinate system. Using the camera's focal length and principal point coordinates, these image line segments are projected onto a coordinate system with the robot as the origin to obtain candidate road edge line segments.

[0032] For 3D LiDAR point cloud data, ground segmentation is performed using filters to separate ground points from non-ground points. Non-ground points are typically considered potential obstacles or road edges. Point cloud segments belonging to ground boundaries are clustered, and straight lines are fitted to the clustered point cloud clusters to obtain line segment representations in 3D space. These line segments are then projected onto a 2D horizontal plane to obtain corresponding candidate road edge segments. The candidate road edge segments from the image data and the point cloud data are combined to obtain a set of edge segments.

[0033] It should be noted that by identifying candidate road edge segments and constructing an edge segment set, the complexity of the data and the computational burden of processing can be reduced. The straight line segments obtained by fitting can repair small-scale noise, gaps or discontinuities in sensor data. The resulting road edge segments are more stable and continuous than those obtained by directly using the original edge points or unfitted point cloud clusters, thus improving the stability and anti-interference ability of road edge representation.

[0034] Specifically, the edge coordinates of edge segments are analyzed, and edge segments whose coordinates are less than a preset distance threshold from the robot's coordinates are selected to construct road constraints. By removing road edge information that is far from the robot, limited computing power can be concentrated on analyzing and processing the local environment most relevant to the robot's current state, improving the efficiency of subsequent processing and avoiding interference from false or incomplete detections on constraint construction. In real-world scenarios, the sensor's field of view may contain irrelevant geometric features such as the edges of multiple parallel channels and the edges of obstacles at distant points. Distance filtering ensures that the constructed road constraints are always based on the robot's most direct local travel space, enabling the positioning and control system to respond quickly and accurately to the near-field environment, improving the system's real-time performance and environmental adaptability.

[0035] Furthermore, the edge coordinates of the edge segments are analyzed, and edge segments whose distance from the robot's coordinates is less than a preset distance threshold are selected from the set of edge segments. Road constraints are then constructed, including: S301. Analyze the edge coordinates of the edge segments, and filter out the edge segments in the edge segment set whose midpoint coordinates are less than the robot coordinates by a preset distance threshold, to obtain the first segment set. S302. Analyze the movement curvature of the corresponding position point based on the robot's current position, and select edge line segments from the first line segment set whose absolute value of the difference between the line segment curvature and the movement curvature is less than a preset curvature threshold to obtain a second line segment set. S303. Fit and connect the edge line segments on both sides in the second line segment set to obtain the first boundary line and the second boundary line. S304. Combine the analysis of the first and second boundary lines to determine the road centerline, filter out the movement area based on the robot's real-time position, set the robot's movement direction constraints and movement distance constraints, and obtain the road constraints.

[0036] In this embodiment, each line segment in the edge segment set is defined by the coordinates of its two endpoints. The arithmetic mean of the two endpoint coordinates is calculated to obtain the midpoint coordinates of each line segment, which represents the geometric center of the line segment. The robot's real-time coordinates are obtained through the IMU's attitude information, representing the robot's real-time coordinate position. The Euclidean distance between the midpoint coordinates of each line segment and the robot's coordinates is calculated sequentially. A distance threshold is set based on the robot's physical size, typical operating speed, and the width of the passageway in the application scenario. For example, in an indoor warehouse scenario, considering the robot's width and necessary safety margin, the distance threshold is set to 5 meters; in a more open park road, the distance threshold is set to 8 meters. The calculated Euclidean distance is compared with the distance threshold, and all line segments with Euclidean distances less than the distance threshold are filtered out to obtain the first line segment set. Edge line segments that are too far away and have little impact on the current pose estimation and control decision are removed.

[0037] It should be noted that the edge segment set contains a large number of edges from the far end of the corridor, the opposite passage, or unrelated walls. This data will increase the unnecessary computational load and introduce interference information that is inconsistent with the geometric features of the local passage where the robot is currently located. By setting a distance threshold to filter and obtain the first set of segments, we can ensure that the system analyzes the robot's near-field environment, reduce the amount of data processed, eliminate false detections or irrelevant structures at a distance, improve data quality, and enhance the robustness of the entire localization and navigation system in complex spaces.

[0038] Specifically, by analyzing the robot's motion trajectory, the instantaneous curvature of the motion path at the current position is estimated using a quadratic polynomial to fit adjacent position points. This yields the robot's current position curvature, reflecting the degree of path curvature. The curvature is close to zero during straight-line motion, and negative / positive values ​​are obtained when turning left / right. For each edge segment in the first set of line segments, the curvature is calculated. Shorter straight-line segments can be considered to have zero curvature; for longer segments, the curvature is estimated by fitting coordinate points, thus obtaining the corresponding segment curvature.

[0039] Preferably, for each line segment, the absolute value of the difference between the line segment curvature and the robot's movement curvature is calculated. A curvature threshold is set based on sensor noise, the smoothness of road curvature, and the robot's steering sensitivity to reflect the maximum allowable curvature difference tolerance; for example, the curvature threshold is set to 0.05. The calculated absolute value of the curvature difference is compared with the preset curvature threshold, and edge line segments with an absolute value of curvature difference less than the curvature threshold are selected to form a second set of line segments. These line segments are geometrically highly consistent with the robot's current motion trend.

[0040] It's important to note that in structured environments, multiple parallel channel edge lines may appear simultaneously within the robot's sensor field of view. Distance alone cannot distinguish which set of edges belongs to the robot's current path. By comparing the geometric curvature of the edges with the robot's own motion curvature, the robot can automatically match road boundaries consistent with its current trajectory, effectively filtering out interfering edges from parallel or intersecting channels. In curved areas, the edges on both sides of the road have specific curvatures. The curvature of the segments in the selected second set matches the robot's actual trajectory, accurately reconstructing the geometry of the curved road and improving the accuracy of road constraints.

[0041] Specifically, using the robot's current heading as a reference, a local coordinate system is established with the robot as the origin, such as... Figure 2 As shown, the X-axis points directly in front of the robot, and the Y-axis points to the left. For each line segment in the second set of line segments, the Y-coordinate of the midpoint of the line segment in the local coordinate system is calculated. In the local coordinate system, line segments with a positive Y-coordinate at their midpoint are classified as the candidate set for the left side of the road, and line segments with a negative Y-coordinate are classified as the candidate set for the right side of the road. Figure 2The Y-coordinate of line segment 1 is positive, so it is assigned to the candidate set for the left side of the road; the Y-coordinates of line segments 2 and 3 are negative, so they are assigned to the candidate set for the right side of the road. After obtaining the candidate sets for the left and right sides of the road, curve fitting is performed on each set using the least squares method. Taking the candidate set for the left side as an example, the coordinates of the endpoints of all line segments belonging to this set are used as input data points. The least squares method is used to find an optimal fitting curve that minimizes the sum of the squares of the distances from each point on the curve to all the input data points on the left side. The fitted curve is used as the first boundary line. Similarly, the fitting operation is performed on the candidate set for the right side to obtain the second boundary line.

[0042] It should be noted that obtaining the boundary line through least squares fitting can smooth out detection noise and gaps in the line segments, resulting in a continuous and smooth boundary, which enhances the stability of subsequent calculations based on this boundary. Even if only a few discontinuous short line segments are detected for a certain boundary due to occlusion, the fitting process can infer the general direction of the entire boundary based on these fragments, which to some extent completes the missing environmental information, ensures the integrity of the road boundary, and improves the system's robustness to incomplete or occluded data.

[0043] Specifically, for two boundary lines, the midpoint between corresponding sampling points along the road's extension direction is calculated, and these midpoints are connected to obtain the road centerline. The movement area is selected based on the robot's real-time position, and the robot's outline dimensions are obtained. Using the road centerline as a reference, the robot is offset to both sides by a distance, which consists of half the robot's width plus a preset safety margin. For example, if the robot's width is 0.8 meters, the offset on one side could be 0.4 meters + 0.2 meters = 0.6 meters. Within the total road width defined by the two boundary lines, a narrower movement area exclusively for the robot's passage is defined.

[0044] Preferably, based on the movement area, the robot's movement direction constraint and movement distance constraint are set to obtain the road constraint. The movement direction constraint requires that the deviation between the robot's heading angle and the tangent direction of the road centerline at the current robot projection point should be kept within a small directional range. The heading angle is the angle between the robot's forward direction and the X-axis of the environmental coordinate system, and the directional range can be set according to the system's positioning accuracy requirements, for example, ±15 degrees. The movement distance constraint requires that the projection point of the robot's geometric center on the horizontal plane must be within the movement area. Combining the movement direction constraint and the movement distance constraint yields the road constraint.

[0045] It should be noted that by constructing movement direction constraints and movement distance constraints, accurate constraints are provided for the robot localization process. When the robot pose estimation violates these constraints, the robot pose is corrected back, which can effectively suppress localization drift. The movement area accurately determines the robot's safe passage space, ensuring that the generated trajectory does not exceed the movement area, thus enhancing the safety of robot operation. This can improve the accuracy, safety, and system reliability of mobile robots operating autonomously in structured roads.

[0046] Furthermore, the visual odometry information is enhanced according to road constraints. The enhanced visual odometry information is then coupled with motion data and environmental data for analysis. This analysis examines the robot's spatial constraints and state relative to the stationary object, yielding the first pose information, including: S401. Enhance the visual odometer information according to road constraints, extract feature points from the visual odometer information, and map them onto the road centerline to obtain the enhanced visual odometer information. S402. The enhanced visual odometry information, motion data, and environmental data are coupled and analyzed to analyze the robot's spatial constraints and state relative to the fixed object, and the first pose information is obtained.

[0047] In this embodiment, visual odometry information is enhanced according to road constraints. Feature points are extracted from the visual odometry information and mapped onto the road centerline to obtain enhanced visual odometry information. By assigning higher weights to feature points closer to the road centerline, the influence of mismatched points outside the road can be suppressed, allowing the visual odometry calculation process to focus on stable data within the road area. By verifying the consistency of road width through boundary feature points, the matching degree between the current visual observation and the road environment can be dynamically perceived. When the road is clear and the matching is good, the visual information is enhanced. When the road boundary is occluded or the visual features are blurred, the weight of the visual information is reduced to avoid erroneous visual information dominating the fusion result, thereby improving the quality of the positioning information data and providing high-quality data input for the data coupling analysis process. This can suppress cumulative drift and improve the robot's positioning accuracy.

[0048] Specifically, the enhanced visual odometry information, motion data, and environmental data are coupled and analyzed to analyze the robot's spatial constraints and state relative to a fixed object, thus obtaining the first pose information. Through data coupling analysis, IMU data, visual data, and motion data can be mutually corrected. By analyzing the fixed object to impose spatial constraints, the accuracy of robot pose estimation and positioning precision can be improved.

[0049] Furthermore, the visual odometry information is enhanced according to road constraints. Feature points are extracted from the visual odometry information and mapped onto the road centerline to obtain enhanced visual odometry information, including: S501. Extract feature points from visual odometry information to obtain the first set of feature points within the road constraints; S502. For each feature point in the first feature point set, calculate the vertical distance between the projection point onto the road and the road centerline, set the corresponding weight according to the vertical distance, calculate the weighted feature points, and obtain the second feature point set. S503. Select the first feature point closest to the first boundary line and the second feature point closest to the second boundary line from the second feature point set; S504. Calculate the enhancement factor by combining the distance between the first feature point and the second feature point; S505. Enhance each feature point in the second feature point set according to the enhancement factor, integrate and optimize the enhanced feature points to obtain enhanced visual odometry information.

[0050] In this embodiment, the visual odometry information includes the three-dimensional coordinates of a series of feature points in the map coordinate system. The road constraints include spatial limitations on movement, including the movement area enclosed by the first boundary line and the second boundary line. The coordinates of the visual feature points are unified to the same coordinate system as the road constraints. The visual odometry outputs the three-dimensional coordinates obtained through triangulation, and this coordinate system is consistent with the coordinate system used to construct the road constraints, so it can be used directly. For each feature point, it is determined whether the projection point on the ground plane is located within the movement area. This is done by calculating the vertical distance from the projection point to the road centerline and comparing it with the half-width of the movement area, where the half-width is the distance from the road centerline to the boundary of the area. If the vertical distance is less than the half-width, the feature point is considered to be within the road constraints; otherwise, the feature point is considered not to be within the road constraints. All feature points within the road constraints are integrated to obtain the first feature point set.

[0051] It should be noted that by selecting the first set of feature points, the impact of invalid and interfering data in subsequent processing can be reduced, thus improving the data quality of the overall analysis process. During operation, visual odometry extracts a large number of texture features from non-road areas, such as walls, ceilings, moving obstacles, and distant trees. These feature points can provide erroneous or noise-introducing constraints for robot motion estimation. By quickly removing feature points that do not meet road constraints, it can be ensured that the points in the first set of feature points come from the road surface or fixed structures close to the road, avoiding a large amount of erroneous or irrelevant visual information from affecting the subsequent localization optimization process and improving the accuracy and stability of the localization system.

[0052] Specifically, the closer a feature point is to the road centerline, the higher the probability that it belongs to the stable surface texture of the road, and the greater the contribution of the observation of the feature point to the localization. Conversely, the farther away the feature point is, the closer it is to the road boundary or even the blurred area outside the boundary, the more its observation is affected by perspective distortion, occlusion, or non-road objects, and its reliability should be reduced. For each feature point in the first feature point set, calculate its vertical projection point on the ground plane, and calculate the vertical distance from the vertical projection point to the road centerline.

[0053] Preferably, after obtaining the vertical distance of each feature point, a corresponding weight value is assigned according to a preset weight allocation function. The weight allocation function satisfies the following: when the vertical distance is 0, the weight value reaches its maximum value of 1; as the vertical distance increases, the weight value monotonically decreases, specifically using a Gaussian function. Weighted feature points are obtained by weighting according to the weights. Each weighted feature point includes the original feature point coordinates, descriptors, and other information. The weighted feature points are then integrated to obtain a second set of feature points.

[0054] It's important to note that on roads, the central region typically offers a wide field of view, stable texture, and minimal occlusion, resulting in high-quality feature points. By decreasing the weights of feature points in high-quality regions with increasing distance, higher importance can be automatically assigned, better reflecting the observation uncertainty distribution of real-world physical scenes. Feature points with higher weights have a greater impact of observation errors on the final pose solution, while those with lower weights have a smaller impact. This adaptively suppresses the negative impact of feature points with poor observation quality, improving the robustness and accuracy of visual odometry information and providing better visual constraints for generating accurate pose estimates.

[0055] Specifically, for the left boundary, all feature points in the second feature point set are traversed, and the vertical distance from the ground projection point of each feature point to the first boundary line is calculated. The feature point with the smallest vertical distance is selected as the first feature point, i.e., the point closest to the left boundary line. For the right boundary, all feature points in the second feature point set are traversed, and the vertical distance from the ground projection point of each feature point to the second boundary line is calculated. The feature point with the smallest distance is selected as the second feature point, i.e., the point closest to the right boundary line. By identifying the first and second feature points, accurate boundary features are provided for the analysis process, input data is provided for calculating the enhancement factor, and the rationality and adaptability of the enhancement processing are improved.

[0056] Furthermore, based on the coordinates of the first and second feature points, the Euclidean distance between their projections on the ground plane is calculated, reflecting the road width observed based on the current visual features, and is used as the observed width. The average distance between the first and second boundary lines is calculated to obtain the expected road width, which is used as the expected width. The ratio of the observed width to the expected width is calculated to obtain the preliminary factor. To ensure that the range of the enhancement factor is reasonable and insensitive to outliers, the preliminary factor is mapped and restricted. The specific mapping rules include: when the preliminary factor is within the interval [0.9, 1.1], the enhancement factor is 1.0; when the preliminary factor deviates from this interval, the enhancement factor decreases linearly from 1.0. For example, when the preliminary factor is less than 0.7 or greater than 1.3, the enhancement factor drops to a lower value, such as 0.3, and the enhancement factor is calculated.

[0057] It should be noted that when occlusion, strong lighting, or severe mismatches occur in the visual odometry in the boundary region, the calculated observation width will differ significantly from the expected width, resulting in a decrease in the enhancement factor. The enhancement factor is used to adjust the weights of all feature points, which can improve the reliability of visual observation data, enhance the system's adaptability and robustness under complex and non-ideal observation conditions, and avoid the risk of a sharp decline in the performance of the entire positioning system due to local visual failures.

[0058] Specifically, each feature point in the second feature point set is iterated over and multiplied by its corresponding enhancement factor to obtain the enhanced feature point. All enhanced feature points are then integrated to obtain the enhanced visual odometry information. By enhancing the feature points through enhancement factors, the system can dynamically reflect the feature information of the current environment. In ideal environments, visual information has a high weight, dominating pose optimization and providing high-precision updates. In harsh environments, the weight of visual information is automatically suppressed, and the system relies more on inertial sensors such as IMUs, maintaining the continuity of positioning and improving the system's positioning accuracy, robustness, and adaptability in complex real-world scenarios.

[0059] Furthermore, the enhanced visual odometry information, motion data, and environmental data are coupled and analyzed to analyze the robot's spatial constraints and state relative to the stationary object, obtaining the first pose information, including: S601. According to the preset time window, perform coupling analysis on the enhanced visual odometry information, motion data and environmental data to obtain the first fusion result. The coupling analysis process includes: performing time alignment on the enhanced visual odometry information, motion data and environmental data within the sliding time window, calculating the motion increment through data integration, calculating the data weight corresponding to the minimum visual projection error, and fusing according to the data weight. S602. Based on the first fusion result and the distance between the robot and the fixed object, analyze the spatial constraints and state of the robot relative to the fixed object to obtain the pose sequence; S603. Combining the pose sequence and road constraints, analyze the deviation between the robot and the road, correct the deviation of the pose sequence, and obtain the first pose information.

[0060] In this embodiment, a fixed-length time window, such as 0.5 seconds, is set according to the system's positioning accuracy requirements and slides along the time axis. Visual data, IMU data, and encoder data are interpolated to a unified timestamp using linear interpolation. This timestamp sequence is aligned with the high-frequency data of the IMU, ensuring that all observations correspond to the same state at the same moment. Within the sliding window, an optimization problem is constructed with the robot's pose at each moment within the window as the state variable. The constraints include: calculating the weighted sum of squares of the reprojection error using the final enhancement weights of each feature point in the enhanced visual odometry information, thus constructing a visual reprojection error constraint; pre-integrating all the original IMU data within the window to obtain probabilistic constraints on the relative motion between adjacent pose states, thus constructing an inertial constraint; and integrating the encoder data to obtain planar displacement constraints, thus constructing a wheel odometry constraint. The visual error term is multiplied by the enhancement weights of its feature points. The IMU pre-integration error term is weighted by its information matrix, which is the inverse of the covariance matrix. The covariance matrix reflects the uncertainty of the error in the state estimate or measurement and the correlation between its components. As the inverse of the covariance matrix, the information matrix is ​​directly used as the weight matrix for this error term in the optimization problem. The encoder error term is also weighted by its uncertainty, which is specifically the variance of the error. The least squares optimizer is used to solve for the state variable that minimizes the sum of all weighted error terms, thus obtaining the optimal pose sequence within the window, which is used as the first fusion result.

[0061] It should be noted that by using a sliding window to jointly optimize observations from multiple consecutive moments, a smooth trajectory is obtained, avoiding potential jumps in estimation based on single frames or filters. This effectively smooths high-frequency sensor noise and suppresses short-term interference. Dynamic weighting enables real-time complementarity of sensor advantages. During optimization, the system automatically trusts more reliable sensor sources. For example, when the robot makes rapid turns causing image blurring, the visual weight is reduced, and the fusion result relies more on the IMU's angular velocity information. When traveling in a straight line with rich texture, high-weighted visual information dominates, accurately correcting IMU drift. This adaptive fusion strategy ensures that the most reliable information sources are fully utilized under any operating conditions, improving the accuracy and robustness of local trajectory estimation.

[0062] Specifically, based on the first fusion result, the observation information of fixed objects detected in real time from the current environmental data, and the preset prior knowledge base of fixed objects (which stores the precise three-dimensional coordinates of each fixed object in the global coordinate system), these fixed objects are detected and identified from camera images or LiDAR point clouds during robot operation. Once identified, the robot's observation value relative to the fixed object is calculated. For monocular cameras, the relative pose is calculated from the image. Based on the robot's pose estimate at the current moment in the first fusion result, combined with the known global coordinates of the fixed objects, the theoretical observation value of the robot to the fixed object is analyzed, including distance, angle, or relative pose. The actual observation value is compared with the theoretical observation value, and a strong spatial constraint error is constructed according to the difference. This strong spatial constraint error is used as a new optimization term and jointly optimized with the optimization problem that generated the first fusion result.

[0063] For example, strong spatial constraint errors are added to the objective function, with their weights set according to the uncertainties in object recognition and measurement. By resolving this extended optimization problem, a pose sequence corrected for global coordinates of the fixed objects is output. Optimizing the robot pose using the position of the fixed objects fixes the robot's pose estimation in the global coordinate system, enabling closed-loop correction and ensuring long-term global accuracy, thus achieving absolute position calibration. In scenarios such as corridors and between similar shelves, visual odometry is prone to mismatches due to scene repetition. Fixed objects, as known features, provide matching points, effectively eliminating errors and improving the system's reliability in repetitive or feature-sparse environments.

[0064] Specifically, for each pose in the pose sequence, the vertical distance from its position point to the road centerline and the angle between its heading angle and the tangent direction of the road centerline at the projection point of that position are calculated to obtain the lateral deviation and heading deviation. The deviation quantifies the degree of fit between the current estimated trajectory and the ideal road. The pose sequence is then corrected based on the lateral deviation and heading deviation, and the road constraint is used as an optimization term to optimize the pose sequence again.

[0065] Preferably, for the latest pose state, an optimization problem is constructed with the following constraints: Penalizing the robot's position for deviation from the road centerline; the greater the deviation, the higher the cost. The cost function can be set as a quadratic function of the lateral deviation, constructing a distance constraint cost term. Penalizing the inconsistency between the robot's heading and the road direction; the greater the heading deviation angle, the higher the cost, constructing a direction constraint cost term. The optimization problem is solved using a least-squares optimizer to obtain the first pose information.

[0066] It should be noted that by optimizing pose through road constraints, we can ensure that the pose information strictly corresponds to the passable area and eliminate pose estimations that exceed the road constraints. In areas where the road is straight, directional constraints can effectively suppress small drifts in the heading angle. In curved areas, distance and directional constraints work together to make the trajectory more closely match the curvature changes of the road centerline, thereby improving the accuracy and stability of the positioning output.

[0067] like Figure 3 As shown, by combining the first pose information and the preset environment map, the robot's pose change trend is predicted to obtain the second pose information, including: S701. Combining the first pose information and the preset environment map, starting from the robot's current position, analyze the curvature and slope information of the corresponding road and select the first desired trajectory. S702. Based on the robot's current speed and power parameters, predict the robot's pose change trend to obtain the second desired trajectory; S703. Compare the deviation between the first expected trajectory and the second expected trajectory, and calculate the trajectory offset; S704. Combining the trajectory offset, the first desired trajectory, and the second desired trajectory, offset correction is performed on each trajectory change point of the robot to obtain the second pose information.

[0068] In this embodiment, the robot's current position is used as the query point. Nearest neighbor search is performed in the road network of the map to find the road point on the nearest road centerline and determine which specific road segment the robot is currently on. Starting from the road point, along the direction of travel of the road centerline, a sequence of centerline points within a future distance is selected. The length of the distance can be determined by multiplying the robot's current speed by a preset time, for example, the preset time is set to 3 seconds. The curvature and slope information corresponding to each point in this intercepted sequence of centerline points are analyzed to obtain the expected position, expected heading, expected curvature, and expected slope that the robot should be in at each moment. These are then integrated to obtain the first expected trajectory.

[0069] It should be noted that by obtaining information on road curvature and slope within the next few seconds of travel, it is possible to predict upcoming road condition changes such as curves and inclines, providing key information input for adjusting speed, attitude, and control strategies in advance. Since the trajectory is directly derived from a pre-precisely surveyed or constructed environmental map, its absolute position is accurate, and its shape strictly conforms to the geometric design of the real road, avoiding the cumulative error or local deviation that exists in fitting a trajectory based on current local perception, thus ensuring the global consistency and high accuracy of the reference trajectory.

[0070] Specifically, taking the current state given by the first pose information as the initial condition, the control command sent to the underlying drive system at the current moment is set to remain unchanged for a period of time in the future. The robot's motion process is analyzed, and numerical integration is performed on the initial condition and the control process. At each small integration time step, the velocity and pose at the next moment are calculated based on the current velocity, control command, and dynamic parameter constraints. This process is repeated until the entire prediction time is completed, resulting in a predicted pose sequence, which serves as the second expected trajectory. This trajectory reflects the most likely real path that the robot will take under the existing control commands and the robot's physical constraints.

[0071] It should be noted that the second expected trajectory reflects the robot's inertial movement in the real environment. For example, even if the map indicates that a turn is needed ahead, if the robot is currently moving straight at high speed and has not yet started to turn, the predicted trajectory will still maintain straight movement in the short term. The predicted trajectory takes into account the robot's maximum acceleration, turning speed and other limits, and is an executable trajectory within the robot's actual capabilities, ensuring the accuracy and feasibility of the prediction results.

[0072] Specifically, by comparing the deviations between the first and second expected trajectories, both are parameterized as functions of time, and samples are taken at the same set of discrete time points to obtain the poses of the first and second expected trajectories, respectively. At each aligned time point, the perpendicular distance from the position point of the second expected trajectory pose to the tangent direction of the first expected trajectory at the first expected trajectory pose is calculated, yielding the distance deviation. The difference in heading angle between the first and second expected trajectory poses is calculated, yielding the direction deviation. The distance and direction deviations are arranged by time to obtain a deviation sequence, which serves as the trajectory offset. By calculating the trajectory offset, the causes of the offset can be analyzed and corresponding corrective measures can be taken.

[0073] Specifically, the second desired trajectory is corrected for deviation based on the trajectory offset. For each time point, the predicted position on the second desired trajectory is moved a corresponding distance along the direction that cancels the distance deviation, resulting in the corrected position. The predicted heading on the second desired trajectory is moved a corresponding angle along the direction that cancels the direction deviation, resulting in the corrected heading. After correcting all sampling points on the second desired trajectory sequentially, a pose sequence adjusted for deviation is obtained, which serves as the second pose information. By correcting the deviation in real time, the magnitude of the deviation during actual operation can be reduced, improving the accuracy and smoothness of robot positioning and tracking.

[0074] Furthermore, based on the second pose information and the preset movement task, the trajectory deviation is analyzed, corresponding correction commands are generated, and the real-time movement of the mobile robot is controlled to perform autonomous positioning and correction for the mobile robot, including: S801. Based on the second pose information and the preset movement task, analyze the trajectory offset and obtain the trajectory deviation. S802. Calculate the control time domain and control parameters according to the trajectory deviation, generate the corresponding correction command, and control the real-time movement of the mobile robot.

[0075] In this embodiment, the second pose information includes a reference trajectory to which the robot will be guided within the next few seconds. The preset movement task clearly defines the robot's long-term goal on a global scale, specifically a global reference path from the starting point to the ending point. Based on the starting point of the second pose information and the robot's desired state at the current moment, the nearest point is found on the global reference path corresponding to the preset movement task. Then, the desired trajectory that matches the second pose information in terms of time length is selected from the path segment after that point.

[0076] Preferably, the trajectory point sequence of the second pose information is time-aligned with the point sequence of the desired mission trajectory to ensure that the comparison is of the desired state at the same future time. At each aligned future time point, the perpendicular distance from the desired position point in the second pose information at that time to the tangent direction of the corresponding point on the desired mission trajectory is calculated to obtain the lateral tracking deviation. The angle difference between the desired heading angle in the second pose information at that time and the path tangent direction of the corresponding point on the desired mission trajectory is calculated to obtain the heading tracking deviation. The calculated lateral tracking deviation and heading tracking deviation are integrated in chronological order to obtain the trajectory deviation sequence.

[0077] It should be noted that by comparing the second pose information with the global movement task point by point in the future time dimension to obtain the trajectory deviation, it reflects the systematic error of the current guiding trajectory relative to the final goal in the future without intervention. This ensures that the ultimate goal of correction is to bring the robot back to the global task path, rather than just the local optimum. While correcting the current deviation, it can eliminate the trend that leads to future deviations and achieve autonomous positioning and correction.

[0078] Specifically, the control time domain and control parameters are calculated according to the trajectory deviation, and corresponding correction instructions are generated to control the real-time movement of the mobile robot. Based on the prediction and optimization of future trajectory deviations, corresponding control instructions are calculated to accurately correct the robot's movement process, so that the robot's actual motion trajectory conforms to the second pose information, reducing tracking errors and improving trajectory tracking accuracy.

[0079] Furthermore, based on the trajectory deviation, the control time domain and control parameters are calculated to generate corresponding correction commands, controlling the real-time movement of the mobile robot, including: S901. Filter out the deviation point with the largest trajectory deviation according to the trajectory deviation, construct the control time domain with the deviation point as the center, and calculate the control parameters corresponding to each position in the control time domain. S902: By combining the control time domain, control parameters, and the robot's real-time status, corresponding correction commands are generated to control the real-time movement of the mobile robot.

[0080] In this embodiment, the trajectory deviation sequence is traversed, and the absolute value of the lateral tracking deviation is used as the main evaluation index. The time point with the largest value is selected as the deviation point, reflecting the moment when the robot will deviate most severely from the expected path without intervention. A control time domain is constructed by extending a fixed time length forward and backward from the deviation point; for example, extending forward and backward by 0.5 seconds each, the control time domain window reflects the problem segment with the most severe deviation. Within the constructed control time domain, discrete sampling is performed along the time axis, for example, one point every 0.05 seconds. For each sampling moment, a set of control parameters is calculated through a predefined parameter mapping relationship. The control parameters define the behavioral intensity or response characteristics that the controller should take at that moment to pull the robot back from the predicted deviation state to the expected trajectory.

[0081] For example, the parameter mapping relationship includes: when the lateral tracking deviation is large and positive, calculating the negative expected front wheel steering angle increment or expected angular velocity; at the same time, assigning a gain coefficient to this control command, which may be appropriately increased as the absolute value of the lateral tracking deviation increases; calculating the corresponding expected control quantity and the corresponding adjustment gain for each future sampling time, and obtaining the control parameters for each position in the control time domain.

[0082] It should be noted that by analyzing the deviation sequence and determining the control time domain around the future moment when the problem is most prominent, the computational efficiency and targeting of control optimization can be improved. Pre-calculating control parameters for each point in the control time domain provides initial values ​​for the optimization process and ensures the timeliness of the correction response.

[0083] Specifically, starting from the current moment, the control time domain is shifted to a future time period starting from the current moment. Within this new optimization window, the state variables are the robot's pose and velocity at each future discrete moment, and the control variables are the control inputs at each future discrete moment.

[0084] Preferably, the cost function of the optimization problem includes: minimizing the tracking error between the robot's predicted trajectory and the second pose information within the optimization window, and minimizing the variation amplitude of the control quantity. The control parameters serve as: the reference target or desired trajectory for the tracking error term in the optimization problem, and the initial settings of the control quantity weight matrix or constraint boundaries. After setting the cost function and robot dynamic constraints, the optimal control input sequence in the future control time domain is obtained by solving the least squares optimization algorithm. The first control quantity in the optimal control input sequence is used as the correction instruction to be executed immediately in the current control cycle and converted into the direct setpoint of the underlying driver.

[0085] For example, for a differential robot, the target speeds of the left and right wheels are calculated; for a steering wheel robot, the target steering angle and target vehicle speed are calculated. Correction commands are sent to the robot's motor controller or steering servo controller, the system state is updated, and based on the new real-time state and the updated trajectory prediction, the optimization process is repeated to achieve continuous rolling time-domain optimization and closed-loop feedback control. By combining real-time state and control time-domain generated optimization commands, tracking errors and external disturbances can be compensated for in a timely manner, enabling the robot's actual motion trajectory to closely and smoothly track the second pose information, achieving stable, autonomous, and high-precision navigation and correction.

[0086] like Figure 4 As shown, a visual odometry-based autonomous localization and correction system for mobile robots is used to implement a visual odometry-based autonomous localization and correction method for mobile robots, including: The road analysis module analyzes road parameters, identifies road edges, and constructs road constraints based on real-time motion and environmental data collected by sensors on the mobile robot. The pose analysis module enhances the visual odometry information according to road constraints, and couples the enhanced visual odometry information with motion data and environmental data to analyze the robot’s spatial constraints and state relative to the fixed object, and obtains the first pose information. The pose prediction module combines the first pose information with a preset environmental map to predict the robot's pose change trend and obtain the second pose information. The positioning and correction module analyzes the trajectory deviation based on the second pose information and the preset movement task, generates corresponding correction commands, and controls the real-time movement of the mobile robot to perform autonomous positioning and correction.

[0087] Example 2: This embodiment uses a specific application scenario—a mobile robot performing pallet handling operations within a warehouse aisle—to illustrate the overall workflow of this technical solution. In this scenario, the aisle is straight, with clearly defined boundaries formed by regular shelves on both sides. Navigation QR codes are affixed to the ground as anchors, and the environmental map has been pre-loaded with the aisle centerline coordinates and geometric attributes.

[0088] After the mobile robot starts, its forward-facing camera captures images at a rate of 30 frames per second, its inertial measurement unit outputs angular velocity and acceleration at a frequency of 100 Hz, and its wheel encoder records the number of wheel rotations in real time. The robot's body width is 0.8 meters, and its current speed is approximately 1. rice / second. The road analysis module begins operation, receiving a 640×480 pixel image frame. After grayscale conversion and Gaussian filtering, edge detection is performed using the Canny operator, and multiple line segments are identified through Hough transform. The pixel coordinates of these line segments in the image, combined with camera intrinsic parameters (e.g., focal length 800 pixels) and the current coarse pose estimated based on encoder data (assuming X=10 meters, Y=1 meter, heading 0 degrees in the local coordinate system), are transformed to world plane coordinates, forming a set of line segments containing possible edges. The midpoint coordinates of each line segment are calculated, and the Euclidean distance is calculated with the currently estimated robot coordinates. A distance threshold of 5 meters is set, and all line segments with a midpoint distance less than 5 meters are filtered out. For example, after filtering, 3 line segments on the left and 2 on the right are obtained. The curvature of the robot's recent trajectory is calculated. Since it is moving straight, the curvature is close to 0, and a curvature threshold of 0.05 is set. Line segments whose own curvature differs from 0 by less than 0.05 are filtered out, and all line segments are retained. Group the line segments on the left into one group and the line segments on the right into another group, and fit two straight lines using the least squares method for each group. These two lines form the left and right boundary lines. Based on these two lines, calculate the center line of the road, assuming it is a straight line with Y = 1.5 meters. Using the robot's current position as the center, and adding a safety margin of 0.2 meters to its physical width, define a strip-shaped movement area from Y = 0.9 meters to Y = 2.1 meters within the left and right boundaries to generate road constraints. These constraints require the robot to remain within this area and its heading to be as parallel to the center line as possible.

[0089] Simultaneously, the visual odometry module extracts and tracks ORB feature points from the preceding image, outputting the 3D coordinate estimates of these feature points; the enhancement processing unit of the pose analysis module receives these feature points and road constraints. Feature points whose coordinates fall within the moving area are selected to form the first feature point set. For each point, the vertical distance from its ground projection point to the road centerline (Y=1.5 meters) is calculated, and a weight is assigned to each point based on a Gaussian function centered on the centerline with a standard deviation of 0.3 meters; the closer the distance, the higher the weight. From the weighted feature point set, the point closest to the left boundary line is identified, assuming its coordinates are (10.1, 0.9), and the point closest to the right boundary line is identified, assuming its coordinates are (10.1, 2.1). The distance between these two points is calculated to be 1.2 meters. This observation width is compared with the nominal channel width of 1.6 meters, and the enhancement factor is calculated to be 0.75. For example, according to the preset mapping relationship, 1.2 / 1.6=0.75 corresponds to an enhancement factor of 0.75. The weights of all feature points are scaled using this factor to obtain the final weighted enhanced visual odometry information. Within a sliding time window of 0.5 seconds, the enhanced visual information, IMU data, and encoder data are time-aligned to construct an optimization problem with the robot's pose within the window as the state variable. The weights for the visual reprojection error term are the enhanced weights described above, while the weights for the IMU pre-integration error and wheel velocimetry error are determined using their inherent sensor noise models. The problem is solved by fusion using an extended Kalman filter to obtain a preliminary pose sequence. The camera recognizes a pre-defined QR code on the ground, corresponding to a fixed object, whose global coordinates are known as (10.5, 1.5, 0). The deviation between the robot's current observation of the QR code and the relative position predicted based on the preliminary pose sequence is calculated. This deviation is then used as a new constraint, which, together with the preliminary pose sequence, is further optimized in conjunction with the road constraint. The road constraint is treated as a soft constraint, penalizing the distance and heading deviation from the centerline. Finally, the accurate first pose information is output, for example (X=10.05 meters, Y=1.1 meters, heading=0.02 radians).

[0090] The pose prediction module receives the first pose information and queries a preset environment map. Starting from the current position (10.05, 1.1), it extracts the road centerline within a 3-second (approximately 3-meter) length along the forward direction. This centerline is recorded as a straight line with 0 curvature and 0 slope on the map, serving as the first expected trajectory. Based on the robot's current speed of 1 m / s, maximum acceleration, and other dynamic parameters, the module predicts the robot's trajectory within the next 3 seconds based on a kinematic model, obtaining the second expected trajectory. Comparing the two, it is found that since the first pose information indicates a 0.05-meter lateral offset (Y=1.1 meters, centerline Y=1.5 meters), the second expected trajectory will be parallel to the centerline but maintain this offset. Calculating the lateral deviation sequence at future time points reveals that the offset stabilizes at around 0.4 meters. The second expected trajectory is pre-corrected, for example, by applying an adjustment at each point on the trajectory that brings the position closer to the centerline by a certain proportion (e.g., gain 0.5), generating a second pose information sequence.

[0091] The positioning and correction module receives the second pose information sequence and the movement task from the upper layer: "Move in a straight line along the channel to X=50 meters." It compares the second pose information with the global path extracted from the task (i.e., a straight line with Y=1.5 meters), analyzing that despite predictive correction, the trajectory still has a residual lateral deviation of approximately 0.2 meters in the future. It identifies the future time point with the largest deviation, for example, 1 second later, and constructs a 1-second control time domain centered on this point. In each control cycle, such as 50 milliseconds, combined with the robot's real-time state, including actual speed and steering angle, a model predictive control algorithm is used to solve an optimization problem in the control time domain: minimizing the tracking error between the predicted trajectory and the second pose information, while constraining the magnitude of control variable changes. After solving, the first control command is taken, for example, calculating that the required fine-tuning of the left wheel speed to increase by 0.05 revolutions per second and the right wheel speed to decrease by 0.05 revolutions per second to reach the tracking target. This correction command is sent to the chassis motor controller, driving the wheels to perform fine adjustments, allowing the robot to smoothly correct towards the road centerline. This process is repeated in each control cycle to achieve continuous, high-precision autonomous positioning and correction.

[0092] 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 method for autonomous positioning and deviation correction of a mobile robot based on visual odometry, characterized in that, include: Based on the motion and environmental data collected in real time by sensors on the mobile robot, road parameters are analyzed to identify road edges and construct road constraints. Feature points are extracted from visual odometry information to obtain the first set of feature points within road constraints; For each feature point in the first feature point set, calculate the vertical distance between the projection point onto the road and the road centerline, set the corresponding weight according to the vertical distance, calculate the weighted feature points, and obtain the second feature point set; Select the first feature point closest to the first boundary line and the second feature point closest to the second boundary line from the second feature point set; The enhancement factor is calculated by combining the distance between the first feature point and the second feature point; Each feature point in the second feature point set is enhanced according to the enhancement factor, and the enhanced feature points are integrated and optimized to obtain enhanced visual odometry information. The enhanced visual odometry information, motion data and environmental data are coupled and analyzed to analyze the robot’s spatial constraints and state relative to the fixed object, and to obtain the first pose information. By combining the first pose information and the preset environment map, the robot's pose change trend is predicted to obtain the second pose information; Based on the second pose information and the preset movement task, the trajectory deviation is analyzed, corresponding correction instructions are generated, and the real-time movement of the mobile robot is controlled to enable the mobile robot to perform autonomous positioning and correction. The calculation of the enhancement factor by combining the distance between the first feature point and the second feature point includes: calculating the Euclidean distance between the projection points of the two feature points on the ground plane based on their coordinates, reflecting the road width observed based on the current visual features, and using this distance as the observed width; calculating the average distance between the first boundary line and the second boundary line to obtain the expected road width, and using this distance as the expected width; calculating the ratio of the observed width to the expected width to obtain the preliminary factor; and mapping and restricting the preliminary factor to make its value range reasonable and insensitive to outliers. The specific mapping rules include: when the preliminary factor is within the interval [0.9, 1.1], the enhancement factor is 1.0; when the preliminary factor deviates from this interval, the enhancement factor decreases linearly from 1.

0.

2. The visual odometry-based mobile robot self-positioning and deviation rectifying method according to claim 1, characterized in that, The process of analyzing road parameters, identifying road edges, and constructing road constraints based on real-time motion and environmental data collected by sensors on the mobile robot includes: Based on the motion data and environmental data collected in real time by the sensors on the mobile robot, the road parameters are analyzed to identify the road edge, and the straight line segments of the road edge are extracted to obtain a set of edge line segments; Analyze the edge coordinates of the edge segments, filter out the edge segments whose edge coordinates are less than a preset distance threshold from the robot coordinates, and construct road constraints.

3. The visual odometry-based mobile robot self-positioning and deviation rectifying method according to claim 2, characterized in that, The analysis of edge coordinates of edge segments is used to filter out edge segments whose distance from the robot's coordinates is less than a preset distance threshold, and to construct road constraints, including: Analyze the edge coordinates of the edge segments, and filter out the edge segments in the edge segment set whose midpoint coordinates are less than the robot coordinates by a preset distance threshold, thus obtaining the first segment set. Based on the robot's current position, the movement curvature of the corresponding position point is analyzed. Edge line segments whose absolute value of the difference between the line segment curvature and the movement curvature is less than a preset curvature threshold are selected from the first line segment set to obtain the second line segment set. In the second set of line segments, the edge line segments on both sides are fitted and connected to obtain the first boundary line and the second boundary line; By combining the analysis of the first and second boundary lines to determine the road centerline, and filtering out the movement area based on the robot's real-time position, the robot's movement direction constraints and movement distance constraints are set to obtain the road constraints.

4. The visual odometry-based mobile robot self-positioning and deviation rectifying method according to claim 1, characterized in that, The enhanced visual odometry information, motion data, and environmental data are coupled and analyzed to analyze the robot's spatial constraints and state relative to a stationary object, obtaining the first pose information, including: According to the preset time window, the enhanced visual odometry information, motion data and environmental data are coupled and analyzed to obtain the first fusion result. The coupling analysis process includes: time alignment of the enhanced visual odometry information, motion data and environmental data within the sliding time window, calculation of motion increment by data integration, calculation of the data weight corresponding to the minimum visual projection error, and fusion according to the data weight. Based on the first fusion result and the distance between the robot and the fixed object, the spatial constraints and state of the robot relative to the fixed object are analyzed to obtain the pose sequence; By combining the pose sequence and road constraints, the deviation between the robot and the road is analyzed, and the pose sequence is corrected to obtain the first pose information.

5. The visual odometry-based mobile robot self-positioning and deviation rectifying method according to claim 1, characterized in that, The process of combining the first pose information and a preset environmental map to predict the robot's pose change trend and obtain the second pose information includes: Combining the first pose information and the preset environment map, starting from the robot's current position, the curvature and slope information of the corresponding road are analyzed to select the first desired trajectory; Based on the robot's current speed and power parameters, predict the robot's pose change trend to obtain the second desired trajectory; Compare the deviation between the first expected trajectory and the second expected trajectory, and calculate the trajectory offset; By combining the trajectory offset, the first desired trajectory, and the second desired trajectory, offset correction is performed on each trajectory change point of the robot to obtain the second pose information.

6. The method for autonomous localization and correction of a mobile robot based on visual odometry according to claim 1, characterized in that, The step of analyzing trajectory deviation based on the second pose information and a preset movement task, generating corresponding correction commands, and controlling the real-time movement of the mobile robot to perform autonomous positioning and correction includes: Based on the second pose information and the preset movement task, the trajectory offset is analyzed to obtain the trajectory deviation; The control time domain and control parameters are calculated based on the trajectory deviation, and corresponding correction commands are generated to control the real-time movement of the mobile robot.

7. The method for autonomous localization and correction of a mobile robot based on visual odometry according to claim 6, characterized in that, The step of calculating the control time domain and control parameters according to the trajectory deviation, generating corresponding correction commands, and controlling the real-time movement of the mobile robot includes: Based on the trajectory deviation, the deviation point with the largest trajectory deviation is selected, and a control time domain is constructed with the deviation point as the center. The control parameters corresponding to each position in the control time domain are calculated. By combining the control time domain, control parameters, and the robot's real-time status, corresponding correction commands are generated to control the real-time movement of the mobile robot.

8. A mobile robot autonomous positioning and correction system based on visual odometry, characterized in that, The method for implementing the visual odometry-based autonomous localization and correction method for mobile robots as described in any one of claims 1 to 7 includes: The road analysis module analyzes road parameters, identifies road edges, and constructs road constraints based on real-time motion and environmental data collected by sensors on the mobile robot. The pose analysis module enhances the visual odometry information according to road constraints, and couples the enhanced visual odometry information with motion data and environmental data to analyze the robot’s spatial constraints and state relative to the fixed object, and obtains the first pose information. The pose prediction module combines the first pose information with a preset environmental map to predict the robot's pose change trend and obtain the second pose information. The positioning and correction module analyzes the trajectory deviation based on the second pose information and the preset movement task, generates corresponding correction commands, and controls the real-time movement of the mobile robot to perform autonomous positioning and correction.

Images