Visual trajectory identification and deviation correction method for mobile robot

Abstract

The invention discloses a visual trajectory recognition and deviation correction method for a mobile robot, which belongs to the technical field of robot navigation and comprises the following steps: firstly, calling sensor parameters, intercepting a target path, converting the target path into an expected trajectory, and then synchronously collecting visual and inertial data; extracting trajectory feature points from double paths of the visual image, assigning confidence, and carrying out calibration, noise reduction and pre-integration on inertial data to solve a pose increment; transverse and course deviations between the robot and an expected trajectory are obtained through inverse perspective transformation, whether visual information is degraded or not is judged according to feature point confidence and the like, the deviations are directly input into a self-adaptive coupling controller when the visual information is normal, the deviations are estimated through extended Kalman filtering and then input into the controller during degradation, and driving wheel correction is output and applied to control parameters; meanwhile, sensor parameters are periodically updated; visual and inertial sensing information is combined, accurate trajectory recognition and deviation self-adaptive correction are achieved, and robustness and accuracy of robot trajectory tracking are improved.

Description

Technical Field

[0001] This invention belongs to the field of robot navigation technology, specifically a method for visual trajectory recognition and deviation correction of mobile robots. Background Technology

[0002] Track tracking and deviation correction are core technologies in the autonomous navigation field of mobile robots, widely used in industrial inspection, warehousing and logistics, and service delivery. The accuracy of recognition and the real-time performance of correction directly determine the robot's operational stability and reliability. Currently, robot track recognition largely relies on a single vision sensor, extracting track information through image feature extraction. However, factors such as changes in lighting, ground occlusion, and missing environmental textures can easily lead to feature point detection failures and a sharp drop in confidence, causing track recognition interruptions and making accurate deviation calculation impossible. Simultaneously, existing track deviation correction methods often employ a single control algorithm, heavily relying on visual sensing and lacking effective multi-source information fusion and degradation processing mechanisms. When visual information fails, continuous deviation prediction and correction are difficult to achieve, making the robot prone to track deviation and loss of control. Furthermore, sensor parameters are often fixed values ​​calibrated offline. Minor changes in camera mounting height and pitch angle during robot operation, as well as drifts in intrinsic parameters and distortion coefficients, can lead to cumulative errors in coordinate transformation and track calculation, further reducing track tracking accuracy. Existing technologies suffer from poor synchronization between visual and inertial sensing data, limited trajectory feature extraction methods, and a lack of consideration for feature point confidence weights in deviation calculations. These issues hinder the robot's adaptability to complex scenarios, and the robustness and accuracy of trajectory tracking fall short of practical application requirements. Therefore, developing a trajectory recognition and deviation correction method that integrates multi-source sensor information, possesses visual information degradation processing capabilities, and can dynamically update sensor parameters has become a key requirement for the development of autonomous navigation technology for mobile robots. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention proposes a method for visual trajectory recognition and deviation correction for mobile robots. First, sensor parameters are retrieved and the target path is captured, converted into the desired trajectory, and then visual and inertial data are acquired simultaneously. Trajectory feature points are extracted from the dual paths of the visual image and assigned confidence values. The inertial data is calibrated, denoised, and pre-integrated to calculate the pose increment. Lateral and directional deviations between the robot and the desired trajectory are obtained through inverse perspective transformation. Based on the feature point confidence values, it is determined whether the visual information has been downgraded. If normal, the deviation is directly input into the adaptive coupling controller. If downgraded, the deviation is estimated through extended Kalman filtering and then input into the controller. The output drive wheel correction is applied to the control parameters, while the sensor parameters are periodically updated. This invention combines visual and inertial sensing information to achieve accurate trajectory recognition and adaptive deviation correction, improving the robustness and accuracy of robot trajectory tracking.

[0004] To achieve the above objectives, the present invention provides the following technical solution:

[0005] A method for visual trajectory recognition and deviation correction of a mobile robot includes:

[0006] S1: Obtain the pre-stored target path information and visual sensor identifier, and retrieve the corresponding sensor parameter set based on the visual sensor identifier;

[0007] S2: Based on the sensor parameter set, the target path information is converted into the desired trajectory, and visual images acquired by the visual sensor and raw inertial data acquired by the inertial measurement unit are collected simultaneously.

[0008] S3: Extract features from the visual image to obtain trajectory feature points and their confidence levels, and perform zero-bias calibration and noise suppression on the original inertial data before pre-integration to calculate the robot pose increment.

[0009] S4: Use the sensor parameter set to perform inverse perspective transformation on the feature points of the trajectory to obtain the lateral deviation and heading deviation between the robot and the desired trajectory.

[0010] S5: If the confidence level of the trajectory feature points in the current visual image is higher than the threshold, and there is no loss of trajectory feature points in multiple consecutive frames, and the lateral deviation is within the preset allowable deviation range, then the lateral deviation and heading deviation between the robot and the desired trajectory are input to the adaptive coupling controller, and the steering angle correction and wheel speed correction of the drive wheels are output; otherwise, it is determined that the visual information is degraded. Based on the robot pose increment and historical pose information, the predicted lateral deviation and heading deviation between the robot and the desired trajectory are obtained and input to the adaptive coupling controller, and the steering angle correction and wheel speed correction of the drive wheels are output.

[0011] S6: Apply the steering angle correction and wheel speed correction to the robot's drive wheel control parameters, and update the sensor parameter set at the same time.

[0012] Specifically, the step of converting the target path information into a desired trajectory based on the sensor parameter set includes:

[0013] A sensor parameter set is obtained, including camera intrinsic parameters, distortion coefficients, installation height, and pitch angle. Based on the camera intrinsic parameters and distortion coefficients in the sensor parameter set, the 3D coordinates of pre-stored path points in the world coordinate system are projected and transformed to map the 3D coordinates to the pixel coordinate system, generating a pixel coordinate sequence. Combining the installation height and pitch angle, an inverse perspective transformation model from the pixel coordinate system to the robot body coordinate system is established. The pixel coordinate sequence is transformed to the robot body coordinate system through the inverse perspective transformation model to obtain a discrete desired trajectory point sequence. Based on the desired trajectory point sequence in the robot body coordinate system, a cubic spline interpolation algorithm is used to generate the desired trajectory function.

[0014] Specifically, feature extraction is performed on the visual image to obtain trajectory feature points and their confidence levels, including:

[0015] S3.1: The first feature extraction path and the second feature extraction path are executed in parallel on the visual image; the first feature extraction path uses a lightweight neural network model based on depthwise separable convolution to perform pixel-level semantic segmentation on the visual image and outputs a pixel region probability map that identifies the trajectory region; the second feature extraction path uses Canny edge detection with adaptive threshold combined with probabilistic Hough transform to extract the set of geometric edge line segments in the current visual image.

[0016] S3.2: Perform connected component analysis and centerline extraction on the pixel region probability map to obtain the first candidate trajectory point sequence based on semantic segmentation. Perform line segment clustering and fitting based on direction and position constraints on the geometric edge line segment set to obtain the second candidate trajectory edge line pair based on geometric edges.

[0017] Specifically, feature extraction of the visual image to obtain trajectory feature points and their confidence levels also includes:

[0018] S3.3: If the overlap between the positions of the first candidate trajectory point sequence and the second candidate trajectory edge pair in the image space is higher than a preset first threshold, then the first candidate trajectory point sequence and the second candidate trajectory edge pair are fused, and the midline of the second candidate trajectory edge pair is used as the final trajectory feature point. At the same time, the confidence level of the trajectory feature point is set to the first confidence level value. If only the first candidate trajectory point sequence is effectively detected, then the first candidate trajectory point sequence is used as the trajectory feature point, and the confidence level is set to the second confidence level value. If only the second candidate trajectory edge pair is effectively detected, then the midline is used as the trajectory feature point, and the confidence level is set to the second confidence level value. If neither the first candidate trajectory point sequence nor the second candidate trajectory edge pair is effectively detected, then it is determined that the trajectory feature point of this frame is missing, and the confidence level is zero.

[0019] Specifically, after performing zero-bias calibration and noise suppression on the original inertial data, pre-integration is performed to calculate the robot pose increment, including:

[0020] The raw angular velocity and raw acceleration data output by the inertial measurement unit are acquired; the raw angular velocity and raw acceleration data are zero-biasedly calibrated using the sliding window averaging method to obtain calibrated angular velocity and acceleration data; a first-order low-pass filter is used to suppress noise in the calibrated angular velocity and acceleration data; within the time interval corresponding to the acquisition time of two adjacent visual images, the noise-suppressed angular velocity and acceleration data are pre-integrated to calculate the robot's relative position increment, relative velocity increment, and relative attitude increment relative to the initial time within the corresponding time interval, which together constitute the robot's pose increment.

[0021] Specifically, the specific steps of S4 include:

[0022] S4.1: Based on the camera intrinsic parameters, back-project the pixel coordinates of the trajectory feature points to the normalized camera coordinate system with the camera optical center as the origin. Based on the installation height and pitch angle, calculate the homography transformation matrix from the normalized camera coordinate system to the robot body coordinate system.

[0023] S4.2: Apply the homography transformation matrix to transform the coordinates of the trajectory feature points in the normalized camera coordinate system to the robot body coordinate system to obtain the lateral coordinates of the trajectory feature points in the robot body coordinate system.

[0024] S4.3: Using the confidence level corresponding to each trajectory feature point as the weight, the lateral coordinates of all trajectory feature points in the robot body coordinate system are weighted and averaged to obtain the lateral deviation between the robot and the desired trajectory.

[0025] S4.4: In the pixel coordinate system, with the vertical pixel coordinates of all trajectory feature points as independent variables and the horizontal pixel coordinates as dependent variables, a fitting straight line is fitted using the least squares method, and the angle between the fitting straight line and the vertical axis of the image is calculated as the image heading angle.

[0026] S4.5: Based on the camera intrinsic parameters and the installation pitch angle, the image heading angle is transformed into the robot body coordinate system to obtain the actual heading angle of the trajectory in the current visual image. The difference between the actual heading angle and the tangent direction angle of the desired trajectory function at the robot's current position is calculated as the heading deviation.

[0027] Specifically, the condition for determining visual information degradation as described in S5 is based on a comprehensive evaluation of the confidence sequence and geometric continuity of the trajectory feature points across multiple frames of visual images within a dynamic sliding window, specifically including:

[0028] S5.1: Calculate the moving average confidence score for the N frames preceding the current time t. ;

[0029] S5.2: Calculate the position offset of the feature points of the current frame's trajectory in the robot's body coordinate system after coordinate transformation compared to the feature points of the previous frame's trajectory. With direction offset ;

[0030] S5.3: If And the number of consecutive frames M is greater than the set first frame threshold. If so, it is determined to be a Category I persistent low confidence downgrade, where, This represents the first confidence threshold, and ;

[0031] S5.4: If ,but or If the deviation exceeds the reasonable offset threshold dynamically calculated based on the robot's current speed, it is classified as a second-type geometric abrupt downgrade. Indicates the confidence level of the current frame. This represents the second confidence threshold, and ;

[0032] S5.5: If the confidence level is within K consecutive frames... and The number of oscillations exceeds a preset oscillation threshold. If so, it is determined to be a type III confidence oscillation downgrade;

[0033] S5.6: If any of the conditions in S5.3, S5.4, or S5.5 are met, the visual information degradation flag is triggered.

[0034] Specifically, the step of obtaining the predicted lateral deviation and heading deviation between the robot and the desired trajectory based on the robot pose increment and historical pose information includes:

[0035] When visual information degradation is determined, a state estimation algorithm based on extended Kalman filter is started. The robot's position, heading angle, lateral deviation and heading deviation at the previous moment are used as the state vector, and the robot's pose increment is used as the control input vector. Based on the robot's kinematic model, a nonlinear state transition equation describing the evolution of the state vector with the control input vector is established. The nonlinear state transition equation is then linearized by a first-order Taylor expansion at the current state estimate to obtain the state transition matrix.

[0036] Using the state transition matrix and the current robot pose increment, a priori estimation is performed on the state vector, and the lateral deviation and heading deviation values ​​included in the prior estimation are output as the predicted lateral deviation and heading deviation.

[0037] Specifically, the adaptive coupling controller is a parallel coupling structure of a proportional-integral-derivative controller and a sliding mode variable structure controller;

[0038] The proportional-integral-derivative controller calculates the basic steering control quantity and the basic wheel speed control quantity based on the weighted sum of the lateral deviation and the heading deviation.

[0039] The sliding mode variable structure controller defines a sliding surface function based on the rate of change of lateral deviation and calculates a robust compensation control quantity; the basic steering control quantity is added to the corresponding steering component in the robust compensation control quantity to obtain the final steering angle correction quantity; the basic wheel speed control quantity is added to the corresponding wheel speed component in the robust compensation control quantity to obtain the final wheel speed correction quantity.

[0040] Specifically, applying the steering angle correction and wheel speed correction to the robot's drive wheel control parameters includes:

[0041] S6.1: Obtain the current steering angle setting value of the robot's steering servo motor and the current wheel speed setting value of the hub drive motor;

[0042] S6.2: Add the steering angle correction amount to the current steering angle setting value to obtain the updated steering angle control command; add the wheel speed correction amount to the current wheel speed setting value to obtain the updated wheel speed control command.

[0043] S6.3: The updated steering angle control command and the updated wheel speed control command are sent to the steering servo motor and the wheel hub drive motor respectively through the robot's underlying drive controller for execution.

[0044] Specifically, the updated sensor parameter set includes:

[0045] S6.4: During robot operation, the camera intrinsic parameter matrix and distortion coefficient vector are periodically recalculated using an online calibration algorithm;

[0046] S6.5: The actual distance between the robot and the ground is measured in real time by a laser rangefinder installed on the robot, and the actual distance is used as the updated installation height value;

[0047] S6.6: The updated camera pitch angle relative to the horizontal plane is calculated by combining the robot pitch angle attitude information calculated by the inertial measurement unit with the fixed installation angle between the camera and the robot body.

[0048] S6.7: Store and update the recalculated camera intrinsic parameter matrix, distortion coefficient vector, updated installation height value, and updated pitch angle into the sensor parameter set.

[0049] Compared with the prior art, the beneficial effects of the present invention are:

[0050] 1. This invention proposes a method for visual trajectory recognition and deviation correction of mobile robots. This method integrates multi-source data from visual sensing and inertial measurement units, employs dual-path feature extraction and confidence assignment and fusion, and combines inverse perspective transformation to accurately calculate the lateral and directional deviations between the robot and the desired trajectory. Simultaneously, multi-dimensional judgment conditions are designed to achieve accurate recognition of visual information degradation. When visual information is normal, the deviation is directly input into the adaptive coupling controller. When degradation occurs, the deviation is estimated by combining extended Kalman filtering with robot pose increment, and supplemented by inertial dead reckoning for localization. This achieves adaptive, full-scene correction of trajectory tracking deviations, effectively solving the problem of recognition failure caused by the susceptibility of single visual sensing to environmental influences, and improving the stability and robustness of robot trajectory tracking.

[0051] 2. This invention proposes a method for visual trajectory recognition and deviation correction for mobile robots. This method generates a smooth and continuous desired trajectory through cubic spline interpolation and adopts an adaptive coupled control structure of PID and sliding mode variable structure controller in parallel. The output steering angle and wheel speed correction can be accurately applied to the robot's drive wheels. At the same time, the sensor parameter set is periodically updated online during operation, and key parameters such as camera intrinsic parameters and installation height are continuously calibrated. This ensures both the accuracy and real-time performance of trajectory tracking control and dynamically adapts to parameter changes during robot operation, reducing errors caused by hardware installation and environmental factors. This improves the versatility and adaptability of the method, enabling mobile robots to achieve high-precision trajectory tracking even in complex scenarios. Attached Figure Description

[0052] Figure 1 This is a schematic diagram of a mobile robot visual trajectory recognition and deviation correction method according to the present invention;

[0053] Figure 2 This is a flowchart illustrating the principle of a mobile robot visual trajectory recognition and deviation correction method according to the present invention. Detailed Implementation

[0054] Example 1:

[0055] Please see Figure 1 and Figure 2 The present invention provides an embodiment of a method for visual trajectory recognition and deviation correction of a mobile robot, comprising the following steps:

[0056] S1: Obtain the pre-stored target path information and visual sensor identifier, and retrieve the corresponding sensor parameter set based on the visual sensor identifier;

[0057] S2: Based on the sensor parameter set, the target path information is converted into the desired trajectory, and visual images acquired by the visual sensor and raw inertial data acquired by the inertial measurement unit are collected simultaneously.

[0058] S3: Extract features from the visual image to obtain trajectory feature points and their confidence levels, and perform zero-bias calibration and noise suppression on the original inertial data before pre-integration to calculate the robot pose increment.

[0059] S4: Use the sensor parameter set to perform inverse perspective transformation on the feature points of the trajectory to obtain the lateral deviation and heading deviation between the robot and the desired trajectory.

[0060] S5: If the confidence level of the trajectory feature points in the current visual image is higher than the threshold, and there is no loss of trajectory feature points in multiple consecutive frames, and the lateral deviation is within the preset allowable deviation range, then the lateral deviation and heading deviation between the robot and the desired trajectory are input to the adaptive coupling controller, and the steering angle correction and wheel speed correction of the drive wheels are output; otherwise, it is determined that the visual information is degraded. Based on the robot pose increment and historical pose information, the predicted lateral deviation and heading deviation between the robot and the desired trajectory are obtained and input to the adaptive coupling controller, and the steering angle correction and wheel speed correction of the drive wheels are output.

[0061] S6: Apply the steering angle correction and wheel speed correction to the robot's drive wheel control parameters, and update the sensor parameter set at the same time.

[0062] Example 2:

[0063] This embodiment uses an AGV mobile robot in a warehousing and logistics scenario as the application object. The robot is equipped with a monocular vision camera as a vision sensor, a six-axis inertial measurement unit (IMU) as an inertial sensing component, a laser rangefinder, a hub drive motor, a steering servo motor, and a low-level drive controller. It is suitable for track tracking operations with marked lines within a warehouse, with an operating speed range of 0.2 m / s to 1.5 m / s. The vision sensor's frame rate is 30 frames per second, and the IMU's data output frequency is 200 Hz. The following provides a detailed illustrative description of each step. All parameter configurations and engineering implementation methods are designed based on the actual application scenario of this AGV mobile robot and can be directly replicated in the track control system of similar mobile robots. Furthermore, it can be adapted and adjusted according to robot parameters in other scenarios such as industrial inspection and service delivery, and does not constitute a limitation on the scope of protection of this application.

[0064] Regarding S1 above:

[0065] In the warehouse AGV mobile robot application scenario of this embodiment, the robot's central processing unit adopts an embedded main control board based on ARM Cortex-A72, the memory is 16G eMMC flash memory, the external server is the warehouse's logistics scheduling server, and the communication with the robot is based on the TCP / IP protocol with a communication baud rate of 115200bps and the data transmission delay is controlled within 50ms.

[0066] The visual sensor identifier is a predefined unique code. In this embodiment, the monocular vision camera is identified as "CAM-AGV-001". This code is bound to the sensor parameter set and stored in the memory of the robot's main control board. The initial values ​​of the sensor parameter set are obtained through offline calibration. Specifically, the camera intrinsic parameters adopt a 3×3 intrinsic parameter matrix, which includes core parameters such as focal length and principal point coordinates; the distortion coefficient is a 5-dimensional vector, covering radial distortion and tangential distortion parameters; the initial value of the camera installation height is 0.6m, which is the vertical distance from the camera's optical center to the warehouse floor; the initial value of the camera pitch angle is 15°, which is the angle between the camera's optical axis and the horizontal plane. This pitch angle is the superposition of the fixed installation angle and the robot's horizontal posture. In the initial state, the robot body is horizontal, so the pitch angle is equal to the fixed installation angle.

[0067] In this embodiment, obtaining the pre-stored target path information is specifically divided into two sub-steps: global path reading and local target path interception. All coordinate calculations are based on a pre-established world coordinate system within the warehouse. This world coordinate system takes the bottom of the pillar in the northwest corner of the warehouse as the origin, the east direction as the positive X-axis, the north direction as the positive Y-axis, and the vertical upward direction as the positive Z-axis. The coordinate unit is meters.

[0068] First, global path reading:

[0069] The pre-planned global path in the warehouse logistics scheduling server consists of multiple 3D path points in the world coordinate system. The spacing of the path points is adapted to different warehouse operation areas. In the densely shelved sorting area, the path point spacing is 0.1m, and in the wide aisle transfer area, the path point spacing is 0.5m. Each 3D path point contains X, Y, and Z axis coordinate values. Since the warehouse floor is a horizontal plane, the Z-axis coordinate value of all path points is 0. After the robot starts, it first reads the global path corresponding to the current task from the logistics scheduling server through the wireless communication module. If the wireless communication is interrupted, the robot directly reads the pre-stored global path backup from its own memory to ensure the robustness of path information acquisition. The read global path is stored in the main control board's memory in the form of an array. The array element is a 3D path point structure, containing fields such as coordinate value, path point number, and path segment type, which facilitates the extraction and retrieval of local target paths.

[0070] Second, local target path interception:

[0071] The robot acquires real-time positioning information through an indoor positioning system. In this embodiment, a UWB indoor positioning system is used, with positioning base stations deployed around the warehouse and positioning tags installed at the center of the robot body. The positioning frequency is 10Hz, and the positioning accuracy is ±0.05m. It can output the robot's three-dimensional positioning coordinates in the world coordinate system in real time, with the Z-axis coordinate value being 0. After acquiring real-time positioning information, the robot extracts a path segment within a predetermined distance from the global path as the target path information to be tracked. This predetermined distance is dynamically adjusted according to the robot's operating speed, following the principle of short distances at low speeds and long distances at high speeds. Specifically, the adjustment rules are as follows: when the operating speed is less than or equal to 0.5m / s, the predetermined distance is 1.0m; when the operating speed is greater than 0.5m / s but less than or equal to 1.0m / s, the predetermined distance is 1.5m; and when the operating speed is greater than 1.0m / s, the predetermined distance is 2.0m. This dynamic adjustment rule design avoids trajectory redundancy caused by extracting excessively long paths at low speeds, while ensuring sufficient path segments are extracted at high speeds, allowing ample reaction time for trajectory conversion and deviation correction.

[0072] The specific implementation logic for local target path interception is as follows: First, based on the robot's real-time positioning coordinates, the closest 3D path point to the robot's current position is found in the 3D path point array of the global path using Euclidean distance calculation, serving as the starting matching point. Then, starting from the starting matching point, the next path points are selected sequentially along the forward direction of the global path until the cumulative distance between the selected path points and the starting matching point reaches a preset predetermined distance, forming a path point subset. Finally, this path point subset is stored in an independent memory buffer as the target path information to be tracked, for use in the expected trajectory conversion in step S2. If the robot's current position exceeds the predetermined distance range of the global path, or if no matching starting path point is found, the robot will trigger a path abnormality warning and reduce its running speed to a low-speed mode of 0.2 m / s until path matching is completed again.

[0073] In this embodiment, the retrieval of sensor parameter sets is based on visual sensor identifiers. The robot's main control board stores a mapping table of multiple sensor parameter sets, with the visual sensor identifier as the primary key and the corresponding sensor parameter set as the value. When the robot activates the visual trajectory recognition function, it first reads the identifier "CAM-AGV-001" of the currently mounted visual sensor, then retrieves the corresponding sensor parameter set through a table lookup operation, and loads this parameter set into the memory of the vision processing module and the coordinate transformation module. If a change in the visual sensor identifier is detected, or an error is detected in the sensor parameter set reading, the robot will disable the trajectory tracking function and issue a sensor fault alarm to ensure the safety of system operation.

[0074] For example, in any work scenario in this embodiment, the robot's real-time positioning coordinates are (50.2, 30.5, 0), its running speed is 0.8 m / s, and the predetermined distance is 1.5 m according to the dynamic adjustment rules. Through Euclidean distance calculation, the path point closest to the current position in the global path is found to be (50.1, 30.4, 0), which is used as the starting matching point. Starting from this starting matching point, subsequent path points (50.2, 30.5, 0), (50.3, 30.6, 0), (50.4, 30.7, 0), (50.5, 30.8, 0), and (50.6, 30.9, 0) are selected along the forward direction. The cumulative distance reaches 1.52 m, approaching the predetermined distance of 1.5 m. This subset of path points, consisting of these six path points, is used as the current target path information. Simultaneously, the visual sensor identifier "CAM-AGV-001" is read, the corresponding sensor parameter set is retrieved, and all operations in step S1 are completed. The execution time of the entire step is controlled within 10ms, which meets the requirements of real-time robot control.

[0075] In this embodiment, all operations in S1 are implemented by the application layer program of the robot's main control board. The program is written in C++ and based on the Linux embedded operating system. It utilizes a multi-threading mechanism to achieve parallel execution of subtasks such as global path reading, real-time positioning information acquisition, local target path interception, and sensor parameter retrieval. Data interaction between threads is achieved through a message queue, ensuring the real-time performance and accuracy of data transmission. The program's execution cycle is synchronized with the visual sensor's frame rate of 33ms, corresponding to 30 frames per second, ensuring that the target path information and sensor parameter set are updated before each visual image frame is acquired. Simultaneously, the program incorporates a robust exception handling mechanism. For anomalies such as path reading failure, lost positioning information, and sensor parameter mismatch, corresponding degradation strategies and alarm mechanisms are designed to improve the robustness of the entire process.

[0076] It should be noted that this embodiment uses a UWB indoor positioning system to obtain the robot's real-time positioning information. In the implementation, laser SLAM, visual SLAM, GPS, and other positioning methods can also be used according to the needs of the application scenario. As long as the robot's positioning coordinates in the world coordinate system can be output in real time, they can be applied to the local target path interception in this step, which does not constitute a limitation of this application. At the same time, the content of the sensor parameter set can be adjusted according to the type of visual sensor. If other visual sensors such as binocular vision cameras or depth cameras are used, baseline distance, depth calibration parameters, and other related content can be added to the sensor parameter set, as long as the parameter set corresponds one-to-one with the visual sensor identifier.

[0077] Regarding S2 above:

[0078] Based on the sensor parameter set, the target path information is converted into the desired trajectory. Simultaneous acquisition of visual images from the vision sensor and raw inertial data from the inertial measurement unit is the core data processing and acquisition step for visual trajectory recognition in this method. The conversion of the desired trajectory provides a reference benchmark for subsequent deviation calculation, while the synchronous acquisition of visual and inertial data ensures the temporal consistency of multi-source sensor data, laying the foundation for subsequent feature extraction and pose calculation. In the warehouse AGV robot application scenario of this embodiment, the execution of S2 is synchronized with the acquisition frame rate of the vision sensor, at 33ms / cycle. All coordinate transformations and data acquisition operations are completed within this cycle, ensuring real-time performance.

[0079] In this embodiment, the conversion of target path information into a desired trajectory based on a set of sensor parameters includes:

[0080] S2.1: Obtain a set of sensor parameters, which includes camera intrinsics, distortion coefficients, installation height, and pitch angle; based on the camera intrinsics and distortion coefficients in the sensor parameter set, perform a projection transformation on the three-dimensional coordinates of the pre-stored path points in the world coordinate system, and map the three-dimensional coordinates to the pixel coordinate system to generate a pixel coordinate sequence.

[0081] Furthermore, the pixel coordinate system takes the upper left corner of the image captured by the visual camera as the origin, the horizontal direction to the right as the positive direction of the u-axis, and the vertical direction downward as the positive direction of the v-axis. The coordinate unit is pixels. In this embodiment, the resolution of the visual camera is 1920×1080 pixels, so the u-axis range is 0~1919 and the v-axis range is 0~1079.

[0082] Furthermore, the specific implementation process of the projection transformation is as follows: First, the three-dimensional path points in the world coordinate system are transformed into three-dimensional coordinates in the camera coordinate system through rigid body transformation. The camera coordinate system has the camera optical center as the origin, the camera optical axis as the positive Z-axis, the horizontal rightward direction as the positive X-axis, and the vertical upward direction as the positive Y-axis. The transformation matrix of the rigid body transformation is determined by the robot's real-time positioning information and the camera's mounting attitude, and includes two parts: a rotation matrix and a translation vector. The rotation matrix is ​​determined by the camera's roll angle, pitch angle, and yaw angle. In this embodiment, the camera's roll angle is 0°, pitch angle is 15°, and yaw angle is... Consistent with the robot's heading angle, the translation vector is determined by the robot's real-time positioning coordinates and the camera's mounting position. Then, the 3D coordinates in the camera coordinate system are projected onto the normalized image plane using the camera intrinsic parameter matrix to obtain normalized 2D coordinates. Next, distortion correction is performed on the normalized 2D coordinates using distortion coefficients to eliminate radial and tangential distortion of the camera lens, resulting in corrected normalized 2D coordinates. Finally, the corrected normalized 2D coordinates are converted back into 2D coordinates in the pixel coordinate system using the camera intrinsic parameter matrix to obtain the pixel coordinates (u, v) of a single path point.

[0083] Furthermore, for all 3D path points in the target path information captured in step S1, a projection transformation operation is performed sequentially to obtain a set of pixel coordinates arranged in the path progression order, forming a pixel coordinate sequence. This pixel coordinate sequence is stored in array form, where each array element is a pixel coordinate structure containing u and v coordinate values ​​and the corresponding world coordinate system path point index, facilitating coordinate inverse transformation and tracing of correspondences. In this embodiment, all matrix operations of the projection transformation are completed by the vision processing module of the robot's main control board, using relevant functions from the OpenCV computer vision library. The computation time is controlled within 5ms, ensuring the real-time performance of the entire step.

[0084] For example, the coordinates of any 3D path point in the target path information are (50.2, 30.5, 0) in the world coordinate system. After rigid body transformation, these coordinates are converted to (0.32, 0.15, 2.10) in the camera coordinate system. Projected onto the normalized image plane using the camera intrinsic parameter matrix, normalized 2D coordinates (0.152, 0.071) are obtained. After distortion coefficient correction, corrected normalized 2D coordinates (0.150, 0.070) are obtained. Finally, these coordinates are converted to (925, 580) in the pixel coordinate system, which is the corresponding position of the path point in the pixel coordinate system. The above operations are performed sequentially on the six 3D path points in the target path information to obtain a pixel coordinate sequence consisting of six pixel coordinates.

[0085] S2.2: Combine the installation height and pitch angle to establish an inverse perspective transformation model from the pixel coordinate system to the robot body coordinate system;

[0086] In this embodiment, the robot body coordinate system has the robot's center as its origin, the robot's forward direction as the positive X-axis, the horizontal leftward direction as the positive Y-axis, and the vertical upward direction as the positive Z-axis. This coordinate system is the core coordinate system for robot motion control, and all deviation calculations and control commands are implemented based on this coordinate system. This step, combined with the installation height and pitch angle from the sensor parameter set, establishes an inverse perspective transformation model from the pixel coordinate system to the robot body coordinate system. The inverse perspective transformation model is a homography transformation model, with the homography matrix as its core, to realize the transformation from two-dimensional coordinates in the pixel coordinate system to two-dimensional coordinates in the body coordinate system. A direct transformation, where the Z-axis coordinate is 0, consistent with the warehouse floor, wherein, Here, the longitudinal coordinate in the vehicle's coordinate system represents the distance traveled in the robot's forward direction. The horizontal coordinate in the vehicle coordinate system represents the distance the robot travels horizontally to the left. The coordinate unit is meters.

[0087] The core of establishing the inverse perspective transformation model lies in solving the homography matrix. The homography matrix is ​​a 3×3 matrix, and its solution process is based on parameters such as the camera's mounting height, pitch angle, and camera intrinsic parameters, while also considering the coordinate transformation relationships between the world coordinate system, camera coordinate system, pixel coordinate system, and vehicle body coordinate system. In this embodiment, the camera's mounting height is 0.6m, and the pitch angle is 15°. Based on the geometric relationships of perspective projection, the distance from any point (u, v) in the pixel coordinate system to the corresponding point in the vehicle body coordinate system is derived. The transformation relationship is determined, and then the homography transformation matrix is ​​solved. During the solution process, the radial and tangential distortions of the camera are ignored because the distortion correction of the path point pixel coordinates has been completed in S2.1, ensuring the accuracy of the transformation.

[0088] The homography matrix is ​​solved by the coordinate transformation module of the robot's main control board, using matrix inversion and multiplication. The result is stored in the memory of the coordinate transformation module for use in coordinate transformation in S2.3. Simultaneously, the homography matrix is ​​adjusted in real-time as the sensor parameter set is updated. If the camera's mounting height or pitch angle changes, the coordinate transformation module will resolve the homography matrix to ensure the accuracy of the inverse perspective transformation model. In this embodiment, the solution time for the homography matrix is ​​controlled within 2ms, meeting real-time requirements. It should also be emphasized that matrix inversion and multiplication are existing technologies in the field and are not inventive solutions of this application; therefore, they will not be elaborated upon here.

[0089] S2.3: The pixel coordinate sequence is transformed to the robot body coordinate system using the inverse perspective transformation model to obtain a discrete desired trajectory point sequence;

[0090] Furthermore, the specific implementation process of S2.3 is as follows: Each pixel coordinate (u, v) in the pixel coordinate sequence is subjected to matrix operation with the homography matrix of the inverse perspective transformation model, directly converting it into two-dimensional coordinates in the vehicle body coordinate system. .

[0091] The above transformation operation is performed sequentially on all pixel coordinates in the pixel coordinate sequence to obtain a set of two-dimensional coordinates in the vehicle body coordinate system arranged in the path forward order, forming a discrete sequence of desired trajectory points. This sequence is stored in array form, where each array element is a two-dimensional coordinate structure in the vehicle body coordinate system, containing... , The coordinate values, along with their corresponding pixel coordinates and world coordinate system path point indices, facilitate interpolation and coordinate tracing. In this embodiment, the matrix operations for coordinate transformation are also performed by the coordinate transformation module, implemented using the OpenCV library. For a sequence containing 6 pixel coordinates, the transformation time is controlled to within 1ms.

[0092] For example, any pixel coordinate in the pixel coordinate sequence is (925, 580). After performing matrix operations with the homography matrix, it is converted into a two-dimensional coordinate (0.5, 0.0) in the vehicle coordinate system. This coordinate indicates that in the vehicle coordinate system, the desired trajectory point is located 0.5m directly in front of the robot with no lateral offset. After converting the six pixel coordinates in sequence, a discrete desired trajectory point sequence composed of six two-dimensional coordinates in the vehicle coordinate system is obtained, with coordinates of (0.1, 0.0), (0.3, 0.0), (0.5, 0.0), (0.7, 0.0), (0.9, 0.0), and (1.1, 0.0).

[0093] S2.4: Based on the desired trajectory point sequence in the robot body coordinate system, a continuous and smooth desired trajectory function is generated using a cubic spline interpolation algorithm.

[0094] Furthermore, the specific steps of S2.4 include: using the longitudinal coordinate of the desired trajectory point in the vehicle body coordinate system as the independent variable and the lateral coordinate as the dependent variable; constructing a cubic polynomial function as a trajectory segment within each interval formed by two adjacent desired trajectory points; to ensure the continuity and smoothness of the entire desired trajectory, constraining the position, first derivative (slope), and second derivative of all adjacent trajectory segments at the connection point to be continuous; solving the linear equation system composed of all constraints to obtain the coefficients of each cubic polynomial function, thereby determining the complete desired trajectory function. The construction and solution process of the cubic polynomial function is prior art in this field and is not an inventive solution of this application, and will not be elaborated upon here.

[0095] In this embodiment, the cubic spline interpolation algorithm is implemented by the numerical calculation module of the robot's main control board. The interpolation algorithm program is written in C++. For the desired trajectory point sequence containing 6 discrete points, 5 piecewise cubic polynomial functions are constructed. A system of linear equations with 20 unknown coefficients is solved using the chasing method, with the computation time controlled within 5ms. The generated desired trajectory function is stored in the memory of the numerical calculation module for use in the deviation calculation of step S4. Simultaneously, the desired trajectory function is refreshed in real time as the target path information is updated, and is regenerated every 33ms to ensure synchronization with the robot's real-time position.

[0096] For example, for the six discrete desired trajectory point sequences obtained in S2.3, five piecewise cubic polynomial functions are constructed with the vertical coordinate as the independent variable and the horizontal coordinate as the dependent variable. The coefficients of each polynomial are obtained by solving the constraint conditions, and the desired trajectory function is generated. The function is continuous and smooth within its domain [0.1, 1.1], and the horizontal coordinate of any point can be obtained using this function. For example, when... When, the result is obtained by solving the function. This indicates that at a distance of 0.6m directly in front of the robot, the lateral offset of the desired trajectory is 0, thus completing the conversion of the desired trajectory.

[0097] Furthermore, visual images acquired by the vision sensor and raw inertial data acquired by the inertial measurement unit are acquired simultaneously, including:

[0098] (1) Send a hardware trigger signal to the vision sensor;

[0099] (2) The visual sensor exposes and acquires a frame of the current visual image at the moment it receives the hardware trigger signal;

[0100] (3) At the same time as sending the hardware trigger signal, record the timestamp output by the inertial measurement unit, and cache the raw angular velocity data stream and raw acceleration data stream for a preset time length starting from the timestamp;

[0101] (4) Pair the acquired current visual image with the original angular velocity data stream and the original acceleration data stream that are aligned with the timestamp to complete the synchronous acquisition of sensor data.

[0102] In this embodiment, the visual sensor is a monocular vision camera with a frame rate of 30 frames / second, a single frame image resolution of 1920×1080, and an image format of JPEG; the inertial measurement unit is a six-axis IMU, which includes a three-axis gyroscope and a three-axis accelerometer, with a data output frequency of 200Hz, and can output raw angular velocity data stream and raw acceleration data stream in real time, with data accuracy of angular velocity ±0.01° / s and acceleration ±0.01m / s².

[0103] In this embodiment, the entire synchronous acquisition process is implemented collaboratively by hardware and software. Hardware triggering ensures the consistency of triggering times, timestamp alignment ensures the accuracy of data pairing, and a preset time-length buffer provides fault tolerance for data pairing, effectively solving the time asynchrony problem caused by the different output frequencies of visual and inertial sensors. The sensor acquisition module program is implemented using bare-metal programming based on an STM32 microcontroller, ensuring the real-time nature of trigger signal transmission and data reading. The entire synchronous acquisition process takes less than 20ms, maintaining synchronization with the acquisition frame rate of the visual sensor.

[0104] For example, at the current system time t=1000ms, the sensor acquisition module sends a hardware trigger signal to the vision sensor, simultaneously records the synchronization timestamp t=1000ms of the inertial measurement unit, and begins buffering 50ms of inertial data. The vision sensor exposes and acquires a visual image frame at t=1000ms, completes image transmission at t=1010ms, and adds a timestamp t=1000ms. The sensor acquisition module selects the inertial data corresponding to t=1000ms from the inertial data buffer, pairs it with the visual image frame, forms a data unit, and stores it in the multi-source sensor data buffer, completing one synchronous acquisition. The time synchronization error of the entire process is 0.5ms, which meets the design requirements.

[0105] It should be noted that this embodiment uses hardware triggering and timestamp alignment to achieve synchronous acquisition. In specific implementation, software synchronization, GPS time synchronization, etc. can also be used. As long as the time synchronization error between the visual image and the inertial data can be guaranteed to be within an acceptable range, such as less than or equal to 5ms, it can be applied to this step and does not constitute a limitation of this application. At the same time, the preset time length of the buffer can be adjusted according to the output frequency of the inertial measurement unit and the acquisition frame rate of the visual sensor, as long as the buffered data contains the inertial data at the time of visual image acquisition.

[0106] Regarding S3 above:

[0107] Ground visual images in warehouse scenarios often present challenges such as complex backgrounds, varying lighting, and worn landmark lines. Single feature extraction methods are prone to issues like feature point detection failures, missed detections, and false detections. Therefore, this embodiment employs a dual-path parallel feature extraction, feature fusion, and confidence assignment approach to extract trajectory features from visual images. By combining semantic segmentation using lightweight neural networks with geometric edge detection from traditional machine vision, accurate trajectory feature recognition is achieved. Simultaneously, confidence assignment quantitatively evaluates the feature extraction results, improving the robustness of feature extraction. The entire feature extraction process is completed by the robot's main control board's vision processing module, implemented using a deep learning framework and the OpenCV machine vision library. Processing a 1920×1080 resolution image frame takes less than 20ms, meeting real-time requirements.

[0108] Feature extraction is performed on the visual image to obtain trajectory feature points and their confidence scores, including:

[0109] S3.1: The first feature extraction path and the second feature extraction path are executed in parallel on the visual image; the first feature extraction path uses a lightweight neural network model based on depthwise separable convolution to perform pixel-level semantic segmentation on the visual image and outputs a pixel region probability map that identifies the trajectory region; the second feature extraction path uses Canny edge detection with adaptive threshold combined with probabilistic Hough transform to extract the set of geometric edge line segments in the current visual image.

[0110] For the first feature extraction path:

[0111] In this embodiment, to address the limited computing resources of the embedded platform of the mobile robot, a lightweight neural network model based on depthwise separable convolution is designed for pixel-level semantic segmentation of visual images. This lightweight neural network model reduces the number of parameters and computational load while ensuring semantic segmentation accuracy, and can run in real time on the embedded platform of the robot's main control board. The lightweight neural network model uses MobileNetV2 as the backbone network, adds a feature pyramid network for multi-scale feature fusion, and finally outputs a pixel region probability map with the same resolution as the input image through an upsampling layer. The training dataset for the model is a ground track image dataset in a warehouse scenario, containing 10,000 track images with different lighting, wear levels, and backgrounds. All images are pixel-level labeled, with the label categories being track and background.

[0112] The specific implementation process of pixel-level semantic segmentation is as follows: First, the visual image is preprocessed, including image scaling, normalization, and color gamut conversion. The 1920×1080 resolution image is scaled down to 512×288 resolution to reduce the computational load. At the same time, the RGB color gamut is converted to the YUV color gamut to improve the robustness of the model to changes in illumination. Then, the preprocessed image is input into a lightweight neural network model. The lightweight neural network model extracts multi-scale semantic features of the image through structures such as depthwise separable convolution and bottleneck residual blocks. Shallow detail features and deep semantic features are fused through a feature pyramid network. Finally, the feature map is restored to 1920×1080 resolution through an upsampling layer, and a pixel region probability map that identifies the trajectory region is output. The value range of each pixel in the probability map is [0,1], which represents the probability that the pixel belongs to the "trajectory" category. The closer the value is to 1, the greater the probability that the pixel is a trajectory.

[0113] In this embodiment, the lightweight neural network model is trained using the TensorFlow framework. After training, the model is converted into a TensorRT inference engine and deployed on the vision processing module of the robot's main control board to achieve real-time inference for semantic segmentation. The inference time for a single frame image is controlled within 10ms, and the output pixel region probability map is stored in the memory of the vision processing module.

[0114] For the second feature extraction path:

[0115] The second feature extraction path uses traditional machine vision algorithms. It extracts edge pixels in the image through Canny edge detection with adaptive thresholds, and then detects the set of geometric edge line segments in the image through probabilistic Hough transform. It extracts the edge features of the trajectory from a geometric level, making up for the shortcomings of deep learning semantic segmentation in low-texture and high-noise scenes.

[0116] For Canny edge detection with adaptive thresholds: This embodiment improves upon the traditional dual-threshold Canny edge detection by adopting an adaptive threshold strategy. The high and low thresholds are dynamically adjusted based on the local grayscale features of the image, enhancing the adaptability of edge detection to changes in lighting and complex backgrounds. The adaptive threshold is calculated as follows: First, the visual image is converted to grayscale to obtain a grayscale image. Then, mean filtering is used to smooth the grayscale image and eliminate random noise. Next, the local grayscale mean and variance of the grayscale image are calculated. Based on the local grayscale mean and variance, the high and low thresholds for each pixel are dynamically calculated. The larger the local grayscale mean, the higher the threshold; the larger the local variance, the larger the threshold difference. Finally, Canny edge detection is performed using the dynamically adjusted high and low thresholds to extract edge pixels from the image, forming a binary edge image. In the binary edge image, a pixel value of 255 represents an edge pixel, and 0 represents a non-edge pixel.

[0117] For geometric edge segment extraction using probabilistic Hough transform: Probabilistic Hough transform is an improved line detection algorithm based on standard Hough transform. By randomly sampling edge pixels, it reduces computational load and is suitable for real-time applications. In this embodiment, the binary edge image obtained from Canny edge detection is input into the probabilistic Hough transform algorithm, with reasonable algorithm parameters set: distance resolution of 1 pixel, angle resolution of 0.5°, voting threshold of 100, maximum line gap of 20 pixels, and minimum line length of 50 pixels. Through probabilistic Hough transform, all line segments that meet the parameter requirements are detected from the binary edge image, forming a set of geometric edge segments. This set is stored in array form, where each array element is a line segment structure containing fields such as the coordinates of the two endpoints, length, and direction angle of the line segment, facilitating line segment clustering and fitting.

[0118] In this embodiment, both the adaptive threshold Canny edge detection and the probabilistic Hough transform are implemented based on the OpenCV machine vision library. The processing time for a single frame image is controlled within 8ms. It is executed in parallel with the semantic segmentation of the first feature extraction path. The total processing time is determined by the longer semantic segmentation, which is 10ms, thus meeting the real-time requirements. The extracted geometric edge line segment set is stored in the memory of the vision processing module.

[0119] S3.2: Perform connected component analysis and centerline extraction on the pixel region probability map to obtain the first candidate trajectory point sequence based on semantic segmentation; perform line segment clustering and fitting based on direction and position constraints on the geometric edge line segment set to obtain the second candidate trajectory edge line pair based on geometric edges.

[0120] Furthermore, the process of generating the first candidate trajectory point sequence includes:

[0121] (1) Binarize the probability map of the pixel region and perform threshold segmentation. Set a fixed threshold of 0.7. Pixels with a probability value greater than or equal to 0.7 are identified as track pixels and assigned a value of 255. Pixels with a probability value less than 0.7 are identified as background pixels and assigned a value of 0. The binary image of the track region is obtained, and the track region is separated from the background region.

[0122] (2) Perform connected component analysis on the binary image after threshold segmentation. Use the 8-neighbor connected component analysis algorithm to detect all connected components in the image and calculate the area, bounding rectangle, center coordinates and other features of each connected component. According to the actual size of the track in the warehouse scenario, set the connected component area screening threshold to remove connected components with too small an area, such as less than 500 pixels, and too large an area, such as greater than 10,000 pixels. Retain connected components that meet the track size characteristics and eliminate the interference of background noise and isolated points.

[0123] (3) For each connected component after filtering, the center line is extracted using a thinning algorithm to obtain the center line pixel points of the trajectory. All center line pixel points are sorted according to the forward direction of the trajectory to form the first candidate trajectory point sequence based on semantic segmentation. The first candidate trajectory point sequence is stored in the form of an array. The array elements are pixel coordinate structures containing u and v coordinate values, arranged in the order of the trajectory from near to far.

[0124] In this embodiment, the generation process of the first candidate trajectory point sequence is implemented based on the OpenCV machine vision library, and the processing time is controlled within 3ms. The generated first candidate trajectory point sequence is stored in the feature buffer of the vision processing module.

[0125] Furthermore, the generation process of the second candidate trajectory edge pair includes:

[0126] (1) Based on the directional characteristics of the track in the warehousing scenario, set the directional angle constraint. The directional angle of the track is consistent with the heading angle of the robot, with a deviation range of ±10°. Remove edge segments whose directional angle exceeds this range. At the same time, set the position constraint to retain only the edge segments in the lower part of the image. Since the robot's vision camera faces the ground, the track mainly appears in the lower part of the image. Remove irrelevant edge segments in the upper part of the image to reduce the computational load of subsequent clustering.

[0127] (2) Cluster the filtered edge segments. Use a distance- and direction-based clustering algorithm to group edge segments that are close to each other and have small directional angle deviations into one class. The distance threshold for clustering is 30 pixels and the directional angle threshold is 5°. The track in the warehouse scenario is a single landmark line, so under normal circumstances it will only be clustered into two classes, corresponding to the left and right edges of the track respectively. The distance- and direction-based clustering algorithm is the prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0128] (3) For each edge segment in each class, the least squares method is used to fit a straight line to obtain a fitted straight line. If the number of classes after clustering is 2, the two fitted straight lines are the left and right edge lines of the track, forming the second candidate track edge line pair; if the number of classes after clustering is 1, the fitted straight line is shifted to both sides by a certain distance, such as half the width of the track. In this embodiment, the width of the track is 0.1m, and the corresponding pixel distance is 50 pixels, generating two edge lines to form the second candidate track edge line pair; if the number of classes after clustering is 0, it is determined that no geometric edge features are detected, and the second candidate track edge line pair is empty. The least squares method is the prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0129] Furthermore, the generated second candidate trajectory edge pair is stored in the form of two straight line structures. Each straight line structure contains fields such as the slope, intercept, and coordinates of the start and end points of the line. These are stored in the feature buffer of the vision processing module and correspond to the first candidate trajectory point sequence.

[0130] S3.3: Perform feature fusion and confidence assignment, including: if the overlap between the positions of the first candidate trajectory point sequence and the second candidate trajectory edge pair in the image space is higher than a preset first threshold, then fuse the first candidate trajectory point sequence and the second candidate trajectory edge pair, and use the midline of the second candidate trajectory edge pair as the final trajectory feature point, and set the confidence of the trajectory feature point to a first confidence value; if only the first candidate trajectory point sequence is effectively detected, then use the first candidate trajectory point sequence as the trajectory feature point, and set the confidence to a second confidence value; if only the second candidate trajectory edge pair is effectively detected, then use its midline as the trajectory feature point, and set the confidence to the second confidence value; if neither the first candidate trajectory point sequence nor the second candidate trajectory edge pair is effectively detected, then it is determined that the trajectory feature point of this frame is missing, and the confidence is zero.

[0131] Further, the positional overlap between the first candidate trajectory point sequence and the second candidate trajectory edge pair in the image space is calculated. Specifically, for each pixel in the first candidate trajectory point sequence, the average pixel distance to the two edge lines of the second candidate trajectory edge pair is calculated. Then, the reciprocal of the average pixel distance of all pixels is calculated as the positional overlap. The positional overlap ranges from [0,1]. The smaller the average pixel distance, the closer the positional overlap is to 1, indicating a higher degree of positional matching between the two candidate features. In this embodiment, a preset first threshold of 0.7 is set to determine whether the two candidate features match.

[0132] Furthermore, based on the determination of position overlap, different feature fusion strategies are executed, and corresponding confidence levels are assigned to the trajectory feature points. In this embodiment, the first confidence level is set to 1.0, and the second confidence level is set to 0.5. The higher the confidence level, the more reliable the feature extraction result. The specific feature fusion strategy is as follows:

[0133] (1) If the position overlap is higher than the preset first threshold, such as 0.7, then the two candidate features are matched and fused together. The midline of the second candidate track edge pair is used as the final track feature point. The midline is generated as follows: calculate the midline of the two edge lines in the second candidate track edge pair, extract the pixel points of the midline, sort them according to the track forward direction, and form the final track feature point sequence. At the same time, the confidence of the track feature point is set to the first confidence value, such as 1.0, indicating that the track feature extraction result of the frame image is highly reliable.

[0134] (2) If the second candidate track edge pair is empty and only the first candidate track point sequence is effectively detected, the first candidate track point sequence is directly used as the final track feature point, and the confidence is set to the second confidence value, such as 0.5. This indicates that the track feature extraction result of this frame image is based only on semantic segmentation and the reliability is generally low.

[0135] (3) If the first candidate trajectory point sequence is empty and only the second candidate trajectory edge pair is effectively detected, the midline of the second candidate trajectory edge pair is taken as the final trajectory feature point, and the confidence is set to the second confidence value, indicating that the trajectory feature extraction result of this frame image is only based on geometric edge detection and the reliability is generally low.

[0136] (4) If neither the first candidate track point sequence nor the second candidate track edge pair is effectively detected, it is determined that the track feature points of this frame are missing, the final track feature point sequence is empty, and the confidence level is assigned to 0, indicating that no track features are detected in this frame image.

[0137] For example, during the feature extraction process of any frame of visual image, the pixel region probability map output by the first feature extraction path is processed to obtain the first candidate trajectory point sequence, which contains 120 pixels; the set of geometric edge line segments output by the second feature extraction path is processed to obtain the second candidate trajectory edge pair, which consists of two fitted straight lines; the positional overlap of the two is calculated to be 0.85, which is higher than the first threshold of 0.7, so it is determined to be a match, and the two are fused. The midline of the second candidate trajectory edge pair is taken as the final trajectory feature point, which contains 115 pixels, and the confidence level is assigned to 1.0; In another frame of visual image, due to excessive illumination, geometric edge detection fails, the second candidate trajectory edge pair is empty, and only the first candidate trajectory point sequence is detected, so this sequence is directly used as the final trajectory feature point, and the confidence level is assigned to 0.5; In any frame of visual image, due to severe wear of the trajectory, both semantic segmentation and geometric edge detection fail, both types of candidate features are empty, and it is determined to be a missing trajectory feature point, and the confidence level is assigned to 0.

[0138] After performing zero-bias calibration and noise suppression on the original inertial data, pre-integration is performed to calculate the robot pose increment, including:

[0139] S3.4: Acquire the raw angular velocity and raw acceleration data output by the inertial measurement unit;

[0140] The generated multi-source sensor data units extract raw inertial data paired with the visual images, including raw angular velocity data streams output from the three-axis gyroscope and raw acceleration data streams output from the three-axis accelerometer. In this embodiment, the output frequency of the inertial measurement unit is 200Hz, and the acquisition time interval between two adjacent frames of visual images is 33ms. Therefore, each data unit contains 7 sets of raw inertial data, such as 33ms × 200Hz ≈ 7. Each set of data includes angular velocity and acceleration values ​​for the X, Y, and Z axes, along with an acquisition timestamp.

[0141] The seven sets of raw inertial data were extracted from the multi-source sensor data unit and stored in the inertial data buffer in the form of an array. The array elements are inertial data structures containing fields such as three-axis angular velocity, three-axis acceleration, and timestamp, and are arranged in ascending order of timestamp. The data extraction time was controlled within 0.5ms, which is almost negligible.

[0142] S3.5: The original angular velocity and acceleration data are zero-biased by using the sliding window mean method to obtain the calibrated angular velocity and acceleration data;

[0143] Zero bias error is one of the main systematic errors of inertial measurement units. It manifests as the sensor outputting angular velocity and acceleration that are not zero when the sensor is in a stationary state. Moreover, this error will drift with time and temperature. In this embodiment, the sliding window mean method is used to perform zero bias calibration on the original angular velocity and acceleration data. The zero bias value is calculated in real time and subtracted from the original data to eliminate the influence of zero bias error.

[0144] The specific implementation process of the sliding window mean method is as follows: First, a sliding window is set for each of the three-axis angular velocities and three-axis accelerations, with a window size of 20 sets of data, corresponding to 100ms of output data from the inertial measurement unit. A first-in, first-out (FIFO) mechanism is used to update the data within the window. Then, the average value of the data within each sliding window is calculated in real time and used as the zero bias value at the current moment. The zero bias value of the angular velocity is denoted as... The zero bias value of acceleration is denoted as Finally, the corresponding zero-bias angular velocity value is subtracted from the original angular velocity data, and the corresponding zero-bias acceleration value is subtracted from the original acceleration data to obtain the calibrated angular velocity data. and calibrated acceleration data The calibration formula is: , ,in, This is the raw angular velocity data. This is the raw acceleration data.

[0145] In this embodiment, the size of the sliding window can be adjusted according to the drift characteristics of the inertial measurement unit. The larger the window, the smoother the calculation of the zero bias value, but the worse the real-time performance; the smaller the window, the better the real-time performance, but the weaker the noise resistance. In this embodiment, a window size of 20 sets of data is selected to balance the stability and real-time performance of the zero bias calibration.

[0146] S3.6: Use a first-order low-pass filter to suppress noise in the calibrated angular velocity and acceleration data;

[0147] Furthermore, the transfer function of the first-order low-pass filter is: ,in, The time constant determines the cutoff frequency for filtering out high-frequency noise, and s represents a complex variable that satisfies... Where w is the angular frequency and j is the imaginary unit. In the discrete implementation, the recursive formula is applied to the angular velocity and acceleration data for each axis: ,in, This is the calibrated raw data at time k. This is the filtered output value at time k. This is the filtered output value at time k-1. The filter coefficients, which are related to the sampling period and cutoff frequency, can effectively suppress high-frequency measurement noise from the sensor and provide smoother data for pre-integration.

[0148] S3.7: Within the time interval corresponding to the acquisition time of two adjacent visual images, pre-integration is performed on the noise-suppressed angular velocity and acceleration data to calculate the robot's relative position increment, relative velocity increment, and relative attitude increment relative to the initial time within the corresponding time interval, which together constitute the robot's pose increment.

[0149] Furthermore, the core of pre-integration is to obtain the pose change between two adjacent moments by integrating inertial data without relying on the robot's initial pose. In this embodiment, the acquisition time interval between two adjacent visual images is 33ms. Pre-integration calculates the robot's pose increment within this 33ms based on the noise-suppressed inertial data within this time interval, providing a control input vector for state estimation during subsequent visual information degradation. Pre-integration is divided into three parts: attitude pre-integration, velocity pre-integration, and position pre-integration, which sequentially calculate the relative attitude increment, relative velocity increment, and relative position increment. All calculations are simplified based on the robot's planar motion characteristics, ignoring motion changes in the Z-axis direction and only considering translations in the X and Y axes and rotations around the Z-axis.

[0150] Furthermore, relative attitude increment calculation: using noise-suppressed angular velocity data Based on this, a quaternion integration method is used for attitude pre-integration to avoid the gimbal lock problem caused by Euler angle integration. First, the three-axis angular velocity data are converted into angular velocity components around the Z-axis. Combining the time interval between two adjacent inertial data acquisition moments (e.g., 5ms, corresponding to a 200Hz output frequency), the attitude change quaternion is calculated sequentially for each time interval using the first-order approximate integration formula of quaternions. Then, the attitude change quaternions for all time intervals are multiplied to obtain the total attitude change quaternion for the entire 33ms time interval. Finally, this quaternion is converted into Euler angle form to obtain the relative attitude increment around the Z-axis. The quaternion integration method is existing technology in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0151] Furthermore, the relative velocity increment is calculated using noise-suppressed acceleration data. Based on this, and combined with the calculated relative attitude increments, the acceleration data is transformed into the robot's body coordinate system. The acceleration data from the camera coordinate system is then converted to the robot's body coordinate system. Finally, the acceleration data in the body coordinate system is integrated over time, with an integration interval of 33ms and an integration step size of 5ms, to obtain the relative velocity increments along the X and Y axes. and The unit is m / s.

[0152] Furthermore, the relative position increment is calculated as follows: Based on the calculated relative velocity increment, the velocity data is integrated twice over time. The integration interval and step size are consistent with the velocity integral. The robot's initial motion state is also considered to obtain the relative position increment ΔX along the X-axis and the relative position increment ΔY along the Y-axis, in meters.

[0153] Furthermore, the calculated relative position increment (ΔX, ΔY) and relative velocity increment are... Relative attitude increment The integrated values ​​form the robot's pose increment, which is stored in the memory of the inertial processing module as a structure. This increment is transmitted to both the state estimation module (for deviation prediction during visual information degradation) and the dead reckoning module (as the basis for dead reckoning during visual information degradation). In this embodiment, the pre-integration calculation is implemented by the pre-integration submodule of the inertial processing module. The program is based on a numerical integration algorithm, and the entire calculation process takes less than 3ms. The calculated pose increment error is controlled within the range of ±0.005m for position, ±0.01m / s for velocity, and ±0.1° for attitude, meeting the accuracy requirements of state estimation and dead reckoning.

[0154] For example, within any 33ms time interval, the noise-suppressed angular velocity data is pre-integrated by attitude to obtain a relative attitude increment of 0.5° around the Z-axis; the acceleration data is transformed and integrated by coordinate system to obtain a relative velocity increment of 0.01m / s on the X-axis and 0.002m / s on the Y-axis; the velocity data is integrated twice to obtain a relative position increment of 0.00033m on the X-axis and 0.000066m on the Y-axis. The combined data is the robot pose increment within that time interval, which can be directly used for state estimation and dead reckoning operations.

[0155] Optionally, the pre-integration calculation in this embodiment is simplified based on the robot's planar motion characteristics. In specific implementation, if the robot is suitable for a three-dimensional motion scenario, the simplification can be canceled, and the three-axis angular velocity and three-axis acceleration data can be directly pre-integrated in all dimensions to calculate the pose increment in three-dimensional space. Only the relevant parameters of the integration algorithm need to be adjusted, which does not constitute a limitation of this application. At the same time, the window size of the sliding window averaging method and the cutoff frequency of the first-order low-pass filter can be adapted and adjusted according to the hardware characteristics of different inertial measurement units and the motion characteristics of the robot, taking into account both the accuracy and real-time performance of calibration and filtering.

[0156] The specific steps of S4 include:

[0157] S4.1: Based on the camera intrinsic parameters, back-project the pixel coordinates of the trajectory feature points to the normalized camera coordinate system with the camera optical center as the origin. Based on the installation height and pitch angle, calculate the homography transformation matrix from the normalized camera coordinate system to the robot body coordinate system.

[0158] Furthermore, the core of the inverse perspective transformation is to solve the homography transformation matrix from the normalized camera coordinate system to the robot body coordinate system. This matrix is ​​a 3×3 non-singular matrix, which can realize the direct projection transformation between the two two-dimensional coordinate systems. This step first completes the inverse projection of the trajectory feature points from the pixel coordinate system to the normalized camera coordinate system, and then calculates the required homography transformation matrix by combining the installation height and pitch angle in the sensor parameters.

[0159] Furthermore, the pixel coordinate back-projection process includes: first, reading the camera intrinsic parameter matrix from the sensor parameter set; then, performing a back-projection operation on the pixel coordinates (u,v) of the output trajectory feature points to eliminate projection distortion caused by the camera intrinsic parameters, thus obtaining the two-dimensional coordinates in the normalized camera coordinate system. The formula for back projection is: ,in, The camera intrinsic parameter matrix is ​​3×3. The inverse matrix, and These are the homogeneous coordinates in the pixel coordinate system and the normalized camera coordinate system, respectively.

[0160] Furthermore, the process of solving the homography transformation matrix includes: based on the camera installation height of 0.6m and pitch angle of 15° in the sensor parameter set, and combined with the geometric projection relationship between the normalized camera coordinate system and the robot body coordinate system, the homography transformation relationship between the two coordinate systems is derived. This includes: first, establishing a perspective projection model of the normalized camera coordinate system and the ground coordinate system; using the camera installation height and pitch angle, calculating the geometric parameters in the projection model; then, through the rigid body transformation relationship between the ground coordinate system and the robot body coordinate system, converting the projection model into a transformation model from the normalized camera coordinate system to the body coordinate system; and finally, converting the transformation model into matrix form to obtain a 3×3 homography transformation matrix H.

[0161] In this embodiment, the back projection calculation of pixel coordinates and the solution of the homography transformation matrix are both implemented by the coordinate transformation module based on the OpenCV machine vision library and matrix operation program. For a trajectory feature point sequence containing more than 100 pixels, the back projection time is controlled within 2ms, and the solution of the homography transformation matrix is ​​controlled within 1ms. The obtained homography transformation matrix H will be stored in the memory of the coordinate transformation module for use in coordinate transformation operations. Moreover, this matrix will be re-solved in real time as the sensor parameter set is updated to ensure the accuracy of coordinate transformation.

[0162] S4.2: Apply the homography transformation matrix to transform the coordinates of the trajectory feature points in the normalized camera coordinate system to the robot body coordinate system to obtain the lateral coordinates of the trajectory feature points in the robot body coordinate system.

[0163] Furthermore, the obtained coordinates of the trajectory feature points in the normalized camera coordinate system are transformed to the robot body coordinate system using a homography transformation matrix to obtain the two-dimensional coordinates of the trajectory feature points in the robot body coordinate system. ,in, The vertical coordinate represents the robot's direction of movement. The horizontal coordinate represents the robot's horizontal leftward direction, and the lateral deviation calculation is based on this horizontal coordinate. The specific implementation process involves: normalizing the homogeneous coordinates of the trajectory feature points in the camera coordinate system. Performing matrix multiplication with the homography transformation matrix H yields the homogeneous coordinates in the robot's body coordinate system. Then, the homogeneous coordinates are normalized. and Divide by respectively The two-dimensional Cartesian coordinates of the feature points of the track in the vehicle body coordinate system are obtained. For all pixels in the track feature point sequence, perform the aforementioned back projection and homography transformation operations sequentially to obtain the lateral coordinates of all track feature points in the vehicle coordinate system. All coordinate transformation operations are implemented in batches by the coordinate transformation module. The processing time for more than 100 trajectory feature points is controlled within 2ms. The transformed lateral coordinates will be stored in the cache area of ​​the deviation calculation module.

[0164] S4.3: Using the confidence level corresponding to each trajectory feature point as the weight, the lateral coordinates of all trajectory feature points in the robot body coordinate system are weighted and averaged to obtain the lateral deviation between the robot and the desired trajectory.

[0165] For example, the trajectory feature point sequence of any frame of visual image contains 115 valid feature points, each with a confidence level of 1.0. The horizontal coordinates obtained after coordinate transformation are... All values ​​are between 0.002m and 0.004m. After weighted averaging, the calculated lateral deviation is... A value of 0.003m indicates that the robot's current position is horizontally offset to the left by 0.003m relative to the desired trajectory. In another visual image, due to lighting interference, some feature points have a confidence level of 0.5. After weighted averaging, the calculated lateral deviation is... A value of -0.002m indicates that the robot's current position is horizontally offset to the right by 0.002m relative to the desired trajectory.

[0166] S4.4: In the pixel coordinate system, with the vertical pixel coordinates of all trajectory feature points as independent variables and the horizontal pixel coordinates as dependent variables, a fitting straight line is fitted using the least squares method, and the angle between the fitting straight line and the vertical axis of the image is calculated as the image heading angle.

[0167] Furthermore, the image heading angle is the direction angle of the trajectory in the pixel coordinate system, which is the basis for calculating the actual heading angle in the vehicle body coordinate system. In this embodiment, the least squares method is used to fit the trajectory feature points in the pixel coordinate system to obtain the fitted straight line of the trajectory. Then, the image heading angle is calculated through geometric angles. All operations are completed in the pixel coordinate system without the need for coordinate transformation, ensuring the efficiency of the operation. The specific implementation process is as follows: First, the vertical and horizontal pixel coordinates of the trajectory feature points in the pixel coordinate system are extracted to form a two-dimensional coordinate sample set. Where n is the number of effective trajectory feature points. Let be the vertical pixel coordinates of the i-th valid trajectory feature point. Let be the horizontal pixel coordinates of the i-th valid trajectory feature point; then, the least squares method is used to perform univariate linear regression fitting on the sample set, and the equation of the fitted line is obtained as follows: ,in, Let be the slope of the fitted line, and b be the intercept; finally, based on the slope of the fitted line... Calculate the angle between the fitted line and the vertical axis of the image. This is the image heading angle, and the formula for calculating the angle is: ,in, The arctangent function is used; a positive value indicates that the fitted line is tilted to the right horizontally towards the image, and a negative value indicates that it is tilted to the left horizontally towards the image. In this embodiment, the least squares method for line fitting and angle calculation is implemented by the heading angle submodule of the deviation calculation module. The fitting operation for more than 100 trajectory feature points takes less than 1ms, and the correlation coefficient R² of the fitted line is controlled above 0.95 to ensure the accuracy of the fitting. The accuracy of the calculated image heading angle is controlled within ±0.1°.

[0168] S4.5: Based on the camera intrinsic parameters and the installation pitch angle, the image heading angle is transformed into the robot body coordinate system to obtain the actual heading angle of the trajectory in the current visual image. The difference between the actual heading angle and the tangent direction angle of the desired trajectory function at the robot's current position is calculated as the heading deviation.

[0169] Heading deviation is the angular offset of the robot's current heading relative to the tangent direction of the desired trajectory. It is another core parameter for trajectory deviation correction. In this embodiment, the image heading angle in the pixel coordinate system is first converted into the actual heading angle in the robot's body coordinate system to eliminate the angular deviation caused by the camera's mounting posture. Then, the heading deviation is calculated by comparing it with the tangent direction angle of the desired trajectory. The specific implementation process is as follows:

[0170] (1) Based on the camera's internal pitch angle in the sensor parameter set, such as 15°, and combined with the angular projection relationship between the pixel coordinate system and the vehicle body coordinate system, the image heading angle is determined. By performing coordinate transformation and angle correction, the actual heading angle of the lower orbital line in the robot's body coordinate system is obtained. During the conversion process, the relationship between the camera's optical axis pitch angle and the pixel coordinate system rotation needs to be considered. Angle compensation is performed on the image heading angle. The compensation coefficient is determined by the camera's mounting attitude. In this embodiment, the compensation coefficient is 1.02. The conversion formula is: the actual heading angle is equal to the product of the image heading angle and the compensation coefficient plus the sum of the mounting pitch angle correction. The mounting pitch angle correction is 0° because the camera's initial pitch angle has been calibrated when the coordinate system is established.

[0171] (2) Read the desired trajectory function in the generated vehicle body coordinate system Take the first derivative of this function to obtain the derivative function. The derivative function represents the slope of the tangent line to the desired trajectory at any longitudinal coordinate; then the longitudinal coordinates of the robot's current position are... The origin of the vehicle coordinate system, i.e., the center of the robot body, is substituted into the derivative function. The slope of the tangent to the desired trajectory at the robot's current position is obtained. Then, calculate the tangent direction angle based on the slope. The calculation formula is: ;

[0172] (3) Calculate the actual heading angle Angle with desired tangent direction The difference is used to obtain the heading deviation. A positive value indicates that the robot's current heading is yawed to the left relative to the tangent direction of the desired trajectory, while a negative value indicates yawed to the right. If the desired trajectory is a straight line, then its tangent direction angle is... For a fixed value, no frame-by-frame differentiation is required. If the absolute value of the heading deviation exceeds a preset deviation threshold (set to 5° in this embodiment), it is considered an abnormal heading deviation calculation, and the heading deviation result of the previous frame is used as the reference value for the current frame. In this embodiment, angle conversion, derivative calculation, and heading deviation calculation are all implemented by the deviation calculation module, with the overall time controlled within 1ms. The calculated heading deviation... The accuracy is controlled within ±0.1°.

[0173] For example, the image heading angle of any frame image The value is 0.3°, which is converted to obtain the actual heading angle. The tangent angle of the desired trajectory function at the robot's current position is 0.306°. The calculated heading deviation is 0°. A value of 0.306° indicates that the robot's current heading is veered 0.306° to the left relative to the desired trajectory; the heading angle in another frame is... The value is -0.2°, which is converted to obtain the actual heading angle. The calculated heading deviation is -0.204°. -0.204° means that the robot's current heading is veered 0.204° to the right relative to the desired trajectory.

[0174] Calculated lateral deviation deviation from heading The data will be transmitted to the adaptive coupling controller of S5 as the quantization input data for deviation correction. At the same time, the deviation data will be stored in the memory of the robot's main control board for visual information degradation judgment and state estimation. If the deviation data is abnormal, the deviation calculation fault tolerance mechanism will be triggered, and historical deviation data will be used to complete the data to ensure the continuity of deviation correction.

[0175] It should be noted that the inverse perspective transformation in this embodiment is based on the homography transformation matrix. In the specific implementation process, other transformation matrices such as perspective transformation matrix and projection transformation matrix can also be used to realize the coordinate transformation of the feature points of the trajectory, as long as perspective distortion can be eliminated and the feature points in the pixel coordinate system can be accurately transformed to the robot body coordinate system. This does not constitute a limitation of this application. At the same time, in addition to the weighted average method, the lateral deviation can also be calculated using algorithms such as the median method and the least squares method. The heading angle can also be fitted using Hough transform, RANSAC algorithm, etc., as long as the accuracy and real-time performance of the deviation calculation are guaranteed.

[0176] The condition for determining visual information degradation as described in S5 is based on a comprehensive evaluation of the confidence sequence and geometric continuity of the trajectory feature points in multiple frames of visual images within a dynamic sliding window, specifically including:

[0177] S5.1: Calculate the moving average confidence score for the N frames preceding the current time t. In this embodiment, N is set to 10 frames, corresponding to 0.33s. The moving average can reflect the overall confidence level of recent visual information and avoid misjudgment caused by sudden changes in confidence in a single frame.

[0178] S5.2: Calculate the position offset of the feature points in the current trajectory frame relative to the feature points in the previous frame in the robot's body coordinate system after coordinate transformation. With direction offset The position offset is the average difference between the lateral coordinates of the feature points in two frames, and the direction offset is the difference between the actual heading angles of the feature points in two frames, which is used to evaluate the continuity of the feature geometry.

[0179] S5.3: If And the number of consecutive frames M is greater than the set first frame threshold. If the result is negative, it is classified as a Category I persistent low-confidence downgrade, indicating that the visual information is in a state of low reliability for a long period of time, and the feature extraction results are unreliable. This represents the first confidence threshold, which is set to 0.3 in this embodiment, and the first frame count threshold. Set to 5, and ;

[0180] S5.4: If ,but or If the deviation exceeds the reasonable offset threshold dynamically calculated based on the robot's current speed, it is classified as a second-type geometric abrupt change anomaly, indicating that the visual features exhibit geometric abrupt changes not caused by motion, resulting in false positives and false negatives. Indicates the confidence level of the current frame. This represents the second confidence threshold, which is set to 0.5 in this embodiment. ;

[0181] S5.5: If the confidence level is within K consecutive frames... and The number of violent oscillations exceeds the preset oscillation threshold. If the result is negative, it is determined to be a third type of confidence oscillation downgrade, indicating that the visual information is severely affected by environmental interference and the feature extraction results are extremely unstable. In this embodiment, K=20.

[0182] S5.6: If any of the conditions in S5.3, S5.4, or S5.5 are met, the visual information degradation flag is triggered.

[0183] The process of obtaining the predicted lateral and heading deviations between the robot and the desired trajectory based on the robot pose increment and historical pose information includes:

[0184] A1: When visual information degradation is determined, a state estimation algorithm based on an extended Kalman filter is initiated. The robot's position, heading angle, lateral deviation, and heading deviation at the previous moment are used as the state vector, and the robot's pose increment is used as the control input vector. Based on the robot's kinematic model, a nonlinear state transition equation describing the evolution of the state vector with the control input vector is established. The nonlinear state transition equation is then linearized by performing a first-order Taylor expansion at the current state estimate to obtain the state transition matrix. The Taylor expansion is a prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0185] Furthermore, based on the robot's previous motion state and deviation state, the state vector of the extended Kalman filter is determined. The robot's position, heading angle, lateral deviation from the desired trajectory, and heading deviation in the vehicle coordinate system at the previous moment are integrated into a four-dimensional state vector. The dimension setting fits the motion characteristics of the mobile robot's planar trajectory tracking, eliminating the need to introduce relevant parameters of three-dimensional motion. This simplifies the algorithm's computational load while ensuring the completeness of the state description. The position includes two components: longitudinal position and lateral position. The longitudinal position is the distance between the robot and the reference point along the forward direction, and the lateral position is the robot's offset distance in the horizontal direction perpendicular to the forward direction. The heading angle is the robot's position in the vehicle coordinate system. The reference value of the angle between the robot's current forward direction and the desired trajectory direction is defined as follows: the lateral deviation is the horizontal lateral offset between the robot's center and the desired trajectory; the heading deviation is the difference between the robot's current heading angle and the tangent angle of the desired trajectory. To ensure the accuracy of the calculation and the consistency of units, the unit of all position parameters in the state vector is set to meters, and the unit of all angle parameters is set to radians. The initial values ​​of the longitudinal and lateral positions are determined by the pose calculation results of the last frame when the visual information is normal. The initial value of the heading angle is obtained by accumulating the attitude increments pre-integrated by the inertial measurement unit. The initial values ​​of the lateral and heading deviations are determined by the deviation calculation results of the last frame when the visual information is normal.

[0186] Furthermore, the process of constructing the robot kinematic model includes: based on the planar motion characteristics of the mobile robot, constructing a robot kinematic model that fits the trajectory tracking scenario. The robot kinematic model is a nonlinear model, which is suitable for describing the motion state of low-speed mobile robots such as warehouse AGVs. The construction of the robot kinematic model ignores the robot's vertical motion and rotation around the longitudinal and lateral axes, and only considers the translation in the plane and the rotation around the vertical axis, which fits the actual operation scenario of the robot tracking the ground trajectory. The robot kinematic model clearly defines the evolution of the robot's position and heading angle with pose increments. It determines that longitudinal position changes are jointly determined by longitudinal velocity increments and heading angles, lateral position changes by lateral velocity increments and heading angles, and heading angle changes directly by relative attitude increments. Considering minute slippage during robot motion, motion correction coefficients are introduced into the kinematic model: a longitudinal motion correction coefficient of 0.998, a lateral motion correction coefficient of 0.999, and a heading angle correction coefficient of 1. These correction coefficient values ​​are determined based on actual motion test results of the warehouse AGV robot, effectively compensating for motion errors caused by minute slippage between the robot's wheels and the ground. All parameters of the robot kinematic model are matched to the robot's hardware characteristics; wheel speed response delays of the hub drive motors and steering servo motors are incorporated into the model parameters, ensuring an accurate description of the robot's actual motion state.

[0187] Furthermore, the establishment of the nonlinear state transition equation includes: based on the constructed robot kinematic model, establishing a nonlinear state transition equation describing the evolution of the state vector with the control input vector. The input of the nonlinear state transition equation is the state vector at the previous moment and the control input vector at the current moment, and the output is the predicted state vector at the current moment. The equation form conforms to the planar motion law of the robot. The evolution of the longitudinal position is calculated by the longitudinal position at the previous moment, the longitudinal velocity increment, the sampling time, and the heading angle. The evolution of the lateral position is calculated by the lateral position at the previous moment, the lateral velocity increment, the sampling time, and the heading angle. The evolution of the heading angle is obtained by directly adding the heading angle of the previous moment to the relative attitude increment. The evolution of the lateral deviation is calculated by combining the lateral deviation of the previous moment, the lateral position increment, and the lateral change of the desired trajectory. The evolution of the heading deviation is calculated by combining the heading deviation of the previous moment, the relative attitude increment, and the change of the tangent direction angle of the desired trajectory. The sampling time is fixed at 33.3ms, consistent with the acquisition frame rate of the vision sensor. The lateral change and the change of the tangent direction angle of the desired trajectory are obtained by differentiating the desired trajectory function in the vehicle coordinate system, ensuring that the equations accurately describe the evolution of the deviation state. Meanwhile, process noise is introduced into the nonlinear state transition equation. The process noise follows a Gaussian distribution with a mean of zero. The covariance matrix is ​​a four-dimensional diagonal matrix with diagonal elements set to 0.0001, 0.0001, 0.0001, and 0.0001, respectively, corresponding to the noise variances of longitudinal position, lateral position, heading angle, and deviation state. The parameter settings of the process noise are determined based on the measurement accuracy of the inertial measurement unit, effectively compensating for the effects of sensor measurement errors and model modeling errors.

[0188] Furthermore, the process of selecting the state estimate and determining the Taylor expansion reference point includes: using the robot's previous state estimate as the reference point for the first-order Taylor expansion of the nonlinear state transition equation at the current moment; the previous state estimate is obtained through the update step of the extended Kalman filter; at the initial moment of visual information degradation, the previous state estimate directly adopts the state calculation result of the last frame when the visual information is normal, which is obtained by fusing visual trajectory feature calculation with inertial data pre-integration, thus accurately reflecting the robot's actual motion state; during the continuous phase of visual information degradation, the previous state estimate adopts the optimal estimate obtained after updating the prior estimate of the extended Kalman filter in the previous frame, ensuring the continuity and accuracy of the Taylor expansion reference point; simultaneously, a validity verification mechanism is set for the reference point. If the deviation between the previous state estimate and the actual motion state exceeds a preset threshold (the lateral position deviation threshold is set to 0.01 meters, and the heading angle deviation threshold is set to 0.03 radians), the reference point is immediately corrected using the pose result calculated by inertial dead reckoning, avoiding Taylor expansion linearization errors caused by excessive reference point deviation.

[0189] Furthermore, the process of performing a first-order Taylor expansion of the nonlinear state transition equation includes: firstly, calculating the first-order partial derivative of each component of the nonlinear state transition equation with respect to each element of the state vector to obtain a partial derivative matrix. This matrix reflects the degree of influence of small changes in each element of the state vector on the output of the state transition equation. The calculation of partial derivatives strictly follows the differentiation rules of advanced mathematics and is carried out in combination with the specific form of the robot's kinematic model to ensure the accuracy of the differentiation results. For nonlinear terms such as trigonometric functions and product terms contained in the state transition equation, approximate calculations are performed at the reference point to convert the nonlinear terms into a combination of constant terms and linear terms, completing the linearization of individual components. Then, the linearization results of all components are integrated to obtain the first-order Taylor expansion of the entire nonlinear state transition equation.

[0190] Furthermore, the process of solving the state transition matrix is ​​as follows: First, the defined state vector and the established nonlinear state transition equation are substituted into the partial derivative calculation formula, and the specific value of each partial derivative at the reference point is calculated sequentially. All parameters used in the calculation process are actual engineering values, such as sampling time, motion correction coefficient, and process noise covariance, to ensure the practicality of the calculation results. Then, the calculated partial derivative values ​​are arranged sequentially by row and column, with rows corresponding to the elements of the current state vector and columns corresponding to the elements of the previous state vector, forming a four-dimensional initial matrix. Finally, the initial matrix is ​​validated, and abnormal elements caused by numerical calculations are removed. The threshold for abnormal elements is set to ±10. If there are elements in the initial matrix that exceed this threshold, they are replaced with the mean of adjacent elements to ensure the singularity of the state transition matrix and avoid situations where matrix operations result in no solution or abnormal solutions. The obtained state transition matrix will be directly used in the prior estimation step of the extended Kalman filter. The partial derivative calculation formula is existing technology in this field and is not an inventive solution of this application; therefore, it will not be elaborated upon here.

[0191] A2: Using the state transition matrix and the current robot pose increment, a priori estimation is performed on the state vector, and the lateral deviation value and heading deviation value included in the prior estimation are output as the predicted lateral deviation and heading deviation. The prior estimation is prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0192] The adaptive coupling controller is specifically a parallel coupling structure of a proportional-integral-derivative controller and a sliding mode variable structure controller;

[0193] B1: The proportional-integral-derivative controller calculates the basic steering control quantity and the basic wheel speed control quantity based on the weighted sum of the lateral deviation and the heading deviation. The proportional-integral-derivative controller is existing technology in this field and is not an inventive solution of this application. It will not be described in detail here. In this embodiment, the proportional coefficient is set to 8.0, the integral coefficient is set to 0.5, the derivative coefficient is set to 2.0, and the integral separation threshold is 0.01m to avoid overshoot problems caused by integral saturation.

[0194] B2: The sliding mode variable structure controller defines a sliding surface function based on the rate of change of lateral deviation and calculates a robust compensation control quantity; the basic steering control quantity is added to the corresponding steering component in the robust compensation control quantity to obtain the final steering angle correction quantity; the basic wheel speed control quantity is added to the corresponding wheel speed component in the robust compensation control quantity to obtain the final wheel speed correction quantity.

[0195] Furthermore, the process of the sliding mode variable structure controller calculating the robust compensation control quantity is as follows: the sliding mode variable structure controller calculates the robust compensation control quantity based on the rate of change of the lateral deviation. Define the sliding surface function, where, lateral deviation The first derivative, the sliding surface function in this embodiment Designed as follows: Where c is the sliding surface coefficient, with a value of 10.0, to ensure the reachability and convergence of the sliding surface; then, based on the exponential reaching law, a sliding mode control law is designed, and the robust compensation control quantity is calculated. It includes compensating steering control and compensating wheel speed control. The approach coefficient of the exponential approach law is 5.0 and the attenuation coefficient is 0.1, which effectively suppresses the chattering problem of sliding mode control. The exponential approach law is the prior art in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0196] Furthermore, after determining that the visual information has been degraded, the method further includes initiating dead reckoning based on the inertial measurement unit, including: using the last pose when the visual information is normal as the initial value; continuously performing zero-bias calibration, noise suppression, and pre-integration on the inertial measurement unit data during the visual information degradation period; calculating the robot's continuous pose during the visual information degradation period by accumulating each pose increment; and comparing the calculated pose with the desired trajectory function to obtain the lateral deviation and heading deviation of the dead reckoning.

[0197] The process of applying the steering angle correction and wheel speed correction to the robot's drive wheel control parameters includes:

[0198] S6.1: Obtain the current steering angle setting value of the robot's steering servo motor and the current wheel speed setting value of the hub drive motor;

[0199] In this embodiment, the robot's steering system is driven by a servo motor, and the wheel speed system is driven by a hub motor. Both are controlled in a closed loop by the underlying drive controller, which outputs the current set values ​​in real time. Specifically, the robot's main control board's control calculation module communicates with the underlying drive controller via a CAN bus at a baud rate of 500kbps, with data transmission delay controlled within 1ms. The module sends a parameter reading command to the underlying drive controller. Upon receiving the command, the underlying drive controller immediately feeds back the current steering angle setting of the steering servo motor and the current wheel speed setting of the left and right hub drive motors. In this embodiment, the AGV robot has a differential steering structure, and the wheel speed setting values ​​of the left and right hub motors remain consistent.

[0200] S6.2: Add the steering angle correction amount to the current steering angle setting value to obtain the updated steering angle control command; add the wheel speed correction amount to the current wheel speed setting value to obtain the updated wheel speed control command.

[0201] S6.3: The updated steering angle control command and the updated wheel speed control command are sent to the steering servo motor and the wheel hub drive motor respectively through the robot's underlying drive controller for execution.

[0202] Furthermore, the specific steps of S6.3 include: the underlying drive controller receiving the updated steering angle control command and wheel speed control command; the steering angle control command being calculated by the position loop proportional-integral-derivative controller and generating a pulse width modulation signal to drive the steering servo motor to rotate to the target angle; the wheel speed control command being calculated by the speed loop proportional-integral-derivative controller and generating a pulse width modulation signal to drive the hub drive motor to reach the target speed.

[0203] The updated sensor parameter set specifically includes:

[0204] S6.4: During the operation of the robot, the camera intrinsic parameter matrix and distortion coefficient vector are periodically recalculated using an online calibration algorithm. The online calibration algorithm is existing technology in this field and is not an inventive solution of this application, so it will not be described in detail here.

[0205] Furthermore, the specific implementation process of S6.4 is as follows: In the operating area of ​​the warehouse, multiple fixed checkerboard calibration boards are pre-arranged. The calibration boards are 8×6 in size, with each square measuring 0.05m×0.05m, and are distributed along the robot's running path. During the robot's operation, the vision camera periodically acquires images of the calibration boards, once every 1 second. Then, through an online calibration algorithm, corner detection, sub-pixel refinement, and calibration calculation are performed on the acquired calibration board images to recalculate the camera intrinsic parameter matrix and distortion coefficient vector. Finally, the newly calculated parameters are compared with the original parameters. If the parameter change exceeds a preset threshold (0.1% in this embodiment), the new parameters are used to update the sensor parameter set; otherwise, the original parameters remain unchanged. The online calibration algorithm is implemented by the vision processing module of the robot's main control board. It is based on the improved calibration function of the OpenCV machine vision library. The calibration time for a single calibration board image is controlled within 3ms. The calibration accuracy is consistent with that of offline calibration. The error of the intrinsic parameter matrix is ​​controlled within ±0.1%, and the error of the distortion coefficient is controlled within ±0.001.

[0206] S6.5: The actual distance between the robot and the ground is measured in real time by a laser rangefinder installed on the robot, and the actual distance is used as the updated installation height value;

[0207] In this embodiment, a laser rangefinder sensor is arranged next to the robot's camera mounting position. The measurement direction is vertically downwards, consistent with the vertical height of the camera's optical center. This allows for real-time measurement of the actual distance between the robot and the warehouse floor, i.e., the actual mounting height of the camera. The laser rangefinder sensor has a measurement accuracy of ±0.001m and a measurement frequency of 10Hz, fully meeting the requirements for real-time updates. The specific implementation process is as follows: The laser rangefinder sensor communicates with the robot's main control board via a UART bus at a baud rate of 9600bps, sending the measured actual distance value to the main control board in real time. The parameter management module of the main control board receives the measurement value every 1 second and takes the average of 10 measurements within 1 second as the updated camera mounting height value, avoiding random errors from single measurements. The updated camera mounting height value is compared with the original mounting height value. If the absolute value of the difference exceeds ±0.005m, the updated camera mounting height value is updated to the sensor parameter set; otherwise, the original mounting height value remains unchanged.

[0208] S6.6: The updated camera pitch angle relative to the horizontal plane is calculated by combining the robot pitch angle attitude information calculated by the inertial measurement unit with the fixed installation angle between the camera and the robot body.

[0209] Furthermore, the camera's pitch angle is the angle between the camera's optical axis and the horizontal plane, determined by the robot's pitch angle and the camera's fixed mounting angle. During robot operation, the robot's pitch angle will change slightly due to ground vibrations; therefore, the camera's actual pitch angle needs to be calculated and updated in real time. The specific implementation process is as follows: First, the real-time pitch angle of the robot body calculated by the inertial measurement unit is read. The pitch angle is calculated from the attitude of the inertial data, with an update frequency of 200Hz; then the fixed mounting angle between the camera and the robot body is read. In this embodiment, the angle is 15°, determined by offline calibration and remains fixed; the actual pitch angle of the camera... If the robot body is horizontal, then , Consistent with the initial value; the main control board's parameter management module retrieves 200 values ​​every 1 second. The average of the values ​​is used as the updated pitch angle value. ,Will Compare the pitch angle with the original value. If the absolute value of the difference exceeds ±0.1°, then... Update to the sensor parameter set; otherwise, keep the original pitch angle value unchanged.

[0210] S6.7: Store and update the recalculated camera intrinsic parameter matrix, distortion coefficient vector, updated installation height value, and updated pitch angle into the sensor parameter set.

[0211] In summary, this embodiment provides a mobile robot trajectory tracking method that integrates heterogeneous visual perception, tight vision-inertial coupling, intelligent degradation judgment and switching, adaptive robust control, and online sensor calibration. This method improves the robustness and accuracy of trajectory detection through parallel visual feature extraction and fusion; it provides high-precision pose and deviation when vision is good, and utilizes inertia for short-term reliable prediction when vision degrades; it achieves seamless and smooth switching of control modes by comprehensively judging the quality of visual information through multiple indicators; it balances conventional performance and anti-interference capability through the parallel operation of PID and sliding mode control; and finally, it ensures the long-term accuracy of the system through online sensor parameter updates. The entire method constitutes a complete, adaptive, and highly robust mobile robot visual trajectory tracking and control system solution.

[0212] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments under the guidance of the present invention without departing from the spirit and scope of the present invention. All of these variations are within the protection scope of the present invention.

Claims

1. A method for visual trajectory recognition and deviation correction of a mobile robot, characterized in that, include: S1: Obtain the pre-stored target path information and visual sensor identifier, and retrieve the corresponding sensor parameter set based on the visual sensor identifier; S2: Based on the sensor parameter set, the target path information is converted into the desired trajectory, and visual images acquired by the visual sensor and raw inertial data acquired by the inertial measurement unit are collected simultaneously. S3: Extract features from the visual image to obtain trajectory feature points and their confidence levels, and perform zero-bias calibration and noise suppression on the original inertial data before pre-integration to calculate the robot pose increment. S4: Use the sensor parameter set to perform inverse perspective transformation on the feature points of the trajectory to obtain the lateral deviation and heading deviation between the robot and the desired trajectory. S5: If the confidence level of the trajectory feature points in the current visual image is higher than the threshold, and there is no loss of trajectory feature points in multiple consecutive frames, and the lateral deviation is within the preset allowable deviation range, then the lateral deviation and heading deviation between the robot and the desired trajectory are input to the adaptive coupling controller, and the steering angle correction and wheel speed correction of the drive wheels are output; otherwise, it is determined that the visual information is degraded. Based on the robot pose increment and historical pose information, the predicted lateral deviation and heading deviation between the robot and the desired trajectory are obtained and input to the adaptive coupling controller, and the steering angle correction and wheel speed correction of the drive wheels are output. S6: Apply the steering angle correction and wheel speed correction to the robot's drive wheel control parameters, and update the sensor parameter set at the same time.

2. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 1, characterized in that, The process of converting target path information into a desired trajectory based on a set of sensor parameters includes: A sensor parameter set is obtained, including camera intrinsic parameters, distortion coefficients, installation height, and pitch angle. Based on the camera intrinsic parameters and distortion coefficients in the sensor parameter set, the 3D coordinates of pre-stored path points in the world coordinate system are projected and transformed to map the 3D coordinates to the pixel coordinate system, generating a pixel coordinate sequence. Combining the installation height and pitch angle, an inverse perspective transformation model from the pixel coordinate system to the robot body coordinate system is established. The pixel coordinate sequence is transformed to the robot body coordinate system through the inverse perspective transformation model to obtain a discrete desired trajectory point sequence. Based on the desired trajectory point sequence in the robot body coordinate system, a cubic spline interpolation algorithm is used to generate the desired trajectory function.

3. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 1, characterized in that, Feature extraction is performed on the visual image to obtain trajectory feature points and their confidence scores, including: S3.1: The first feature extraction path and the second feature extraction path are executed in parallel on the visual image; the first feature extraction path uses a lightweight neural network model based on depthwise separable convolution to perform pixel-level semantic segmentation on the visual image and outputs a pixel region probability map that identifies the trajectory region; the second feature extraction path uses Canny edge detection with adaptive threshold combined with probabilistic Hough transform to extract the set of geometric edge line segments in the current visual image. S3.2: Perform connected component analysis and centerline extraction on the pixel region probability map to obtain the first candidate trajectory point sequence based on semantic segmentation. Perform line segment clustering and fitting based on direction and position constraints on the geometric edge line segment set to obtain the second candidate trajectory edge line pair based on geometric edges.

4. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 3, characterized in that, The method further includes extracting features from the visual image to obtain trajectory feature points and their confidence levels, and also includes: S3.3: If the overlap between the positions of the first candidate trajectory point sequence and the second candidate trajectory edge pair in the image space is higher than a preset first threshold, then the first candidate trajectory point sequence and the second candidate trajectory edge pair are fused, and the midline of the second candidate trajectory edge pair is used as the final trajectory feature point. At the same time, the confidence level of the trajectory feature point is set to the first confidence level value. If only the first candidate trajectory point sequence is effectively detected, then the first candidate trajectory point sequence is used as the trajectory feature point, and the confidence level is set to the second confidence level value. If only the second candidate trajectory edge pair is effectively detected, then the midline is used as the trajectory feature point, and the confidence level is set to the second confidence level value. If neither the first candidate trajectory point sequence nor the second candidate trajectory edge pair is effectively detected, then it is determined that the trajectory feature point of this frame is missing, and the confidence level is zero.

5. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 1, characterized in that, After performing zero-bias calibration and noise suppression on the original inertial data, pre-integration is performed to calculate the robot pose increment, including: The raw angular velocity and raw acceleration data output by the inertial measurement unit are acquired; the raw angular velocity and raw acceleration data are zero-biasedly calibrated using the sliding window averaging method to obtain calibrated angular velocity and acceleration data; a first-order low-pass filter is used to suppress noise in the calibrated angular velocity and acceleration data; within the time interval corresponding to the acquisition time of two adjacent visual images, the noise-suppressed angular velocity and acceleration data are pre-integrated to calculate the robot's relative position increment, relative velocity increment, and relative attitude increment relative to the initial time within the corresponding time interval, which together constitute the robot's pose increment.

6. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 2, characterized in that, The specific steps of S4 include: S4.1: Based on the camera intrinsic parameters, back-project the pixel coordinates of the trajectory feature points to the normalized camera coordinate system with the camera optical center as the origin. Based on the installation height and pitch angle, calculate the homography transformation matrix from the normalized camera coordinate system to the robot body coordinate system. S4.2: Apply the homography transformation matrix to transform the coordinates of the trajectory feature points in the normalized camera coordinate system to the robot body coordinate system to obtain the lateral coordinates of the trajectory feature points in the robot body coordinate system. S4.3: Using the confidence level corresponding to each trajectory feature point as the weight, the lateral coordinates of all trajectory feature points in the robot body coordinate system are weighted and averaged to obtain the lateral deviation between the robot and the desired trajectory. S4.4: In the pixel coordinate system, with the vertical pixel coordinates of all trajectory feature points as independent variables and the horizontal pixel coordinates as dependent variables, a fitting straight line is fitted using the least squares method, and the angle between the fitting straight line and the vertical axis of the image is calculated as the image heading angle. S4.5: Based on the camera intrinsic parameters and the installation pitch angle, the image heading angle is transformed into the robot body coordinate system to obtain the actual heading angle of the trajectory in the current visual image. The difference between the actual heading angle and the tangent direction angle of the desired trajectory function at the robot's current position is calculated as the heading deviation.

7. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 1, characterized in that, The condition for determining visual information degradation as described in S5 is based on a comprehensive evaluation of the confidence sequence and geometric continuity of the trajectory feature points in multiple frames of visual images within a dynamic sliding window, specifically including: S5.1: Calculate the moving average confidence score for the N frames preceding the current time t. ; S5.2: Calculate the position offset of the feature points of the current frame's trajectory in the robot's body coordinate system after coordinate transformation compared to the feature points of the previous frame's trajectory. With direction offset ; S5.3: If And the number of consecutive frames M is greater than the set first frame threshold. If so, it is determined to be a Category I persistent low confidence downgrade, in which... This represents the first confidence threshold, and ; S5.4: If ,but or If the deviation exceeds the reasonable offset threshold dynamically calculated based on the robot's current speed, it is classified as a second-type geometric abrupt downgrade. Indicates the confidence level of the current frame. This represents the second confidence threshold, and ; S5.5: If the confidence level is within K consecutive frames... and The number of oscillations exceeds a preset oscillation threshold. If so, it is determined to be a type III confidence oscillation downgrade; S5.6: If any of the conditions in S5.3, S5.4, or S5.5 are met, the visual information degradation flag is triggered.

8. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 1, characterized in that, The process of obtaining the predicted lateral and heading deviations between the robot and the desired trajectory based on the robot pose increment and historical pose information includes: When visual information degradation is determined, a state estimation algorithm based on extended Kalman filter is started. The robot's position, heading angle, lateral deviation and heading deviation at the previous moment are used as the state vector, and the robot's pose increment is used as the control input vector. Based on the robot's kinematic model, a nonlinear state transition equation describing the evolution of the state vector with the control input vector is established. The nonlinear state transition equation is then linearized by a first-order Taylor expansion at the current state estimate to obtain the state transition matrix. Using the state transition matrix and the current robot pose increment, a priori estimation is performed on the state vector, and the lateral deviation and heading deviation values ​​included in the prior estimation are output as the predicted lateral deviation and heading deviation.

9. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 1, characterized in that, The adaptive coupling controller is specifically a parallel coupling structure of a proportional-integral-derivative controller and a sliding mode variable structure controller; The proportional-integral-derivative controller calculates the basic steering control quantity and the basic wheel speed control quantity based on the weighted sum of the lateral deviation and the heading deviation. The sliding mode variable structure controller defines a sliding surface function based on the rate of change of the lateral deviation and calculates a robust compensation control quantity; the basic steering control quantity is added to the corresponding steering component in the robust compensation control quantity to obtain the final steering angle correction quantity; The basic wheel speed control quantity is added to the corresponding wheel speed component in the robust compensation control quantity to obtain the final wheel speed correction quantity.

10. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 1, characterized in that, The process of applying the steering angle correction and wheel speed correction to the robot's drive wheel control parameters includes: S6.1: Obtain the current steering angle setting value of the robot's steering servo motor and the current wheel speed setting value of the hub drive motor; S6.2: Add the steering angle correction amount to the current steering angle setting value to obtain the updated steering angle control command; add the wheel speed correction amount to the current wheel speed setting value to obtain the updated wheel speed control command. S6.3: The updated steering angle control command and the updated wheel speed control command are sent to the steering servo motor and the wheel hub drive motor respectively through the robot's underlying drive controller for execution.

11. The method for visual trajectory recognition and deviation correction of a mobile robot as described in claim 1, characterized in that, The updated sensor parameter set specifically includes: S6.4: During robot operation, the camera intrinsic parameter matrix and distortion coefficient vector are periodically recalculated using an online calibration algorithm; S6.5: The actual distance between the robot and the ground is measured in real time by a laser rangefinder installed on the robot, and the actual distance is used as the updated installation height value; S6.6: The updated camera pitch angle relative to the horizontal plane is calculated by combining the robot pitch angle attitude information calculated by the inertial measurement unit with the fixed installation angle between the camera and the robot body. S6.7: Store and update the recalculated camera intrinsic parameter matrix, distortion coefficient vector, updated installation height value, and updated pitch angle into the sensor parameter set.

Images