Robot accurate alignment method, system, equipment and medium

Abstract

The embodiment of the invention provides a robot precise alignment method and system, electronic equipment and a computer readable storage medium. The method comprises the following steps: acquiring a laser radar point cloud, a visual image and inertial measurement unit data of a robot; performing dynamic point cloud segmentation on the laser radar point cloud, filtering out dynamic interference points to obtain static point cloud, and extracting target point cloud from the static point cloud; constructing a tight coupling fusion model based on the visual image, the target point cloud and the inertial measurement unit data, and solving the tight coupling fusion model to obtain the current pose of the robot; and the deviation between the current pose and a preset standard pose is calculated, and the robot is controlled to move according to the deviation to complete alignment. According to the method, dynamic environment interference is filtered out, and the pose is solved in a multi-sensor tight coupling mode, so that the alignment precision and robustness of the robot in a complex dynamic scene can be effectively improved.

Description

Technical Field

[0001] This application relates to the field of mobile robot navigation and control technology, and in particular to a robot precise positioning method, system, electronic device and computer-readable storage medium. Background Technology

[0002] With the widespread application of mobile robots (AMR / AGV) in warehousing, logistics, manufacturing and other scenarios, the requirements for their docking accuracy, speed and reliability are increasing.

[0003] However, existing technologies generally assume a static environment. In practical applications, the environment is often dynamic, with moving objects such as people and forklifts. These dynamic objects can contaminate the laser point cloud data, leading to feature extraction and matching failures, thus interrupting the alignment process or causing serious pose errors. Furthermore, existing multi-sensor solutions often employ a loosely coupled architecture, where each sensor independently calculates its pose before simple filtering and fusion. This approach fails to fully leverage the complementary advantages of multiple sensors. When a single sensor fails or is interfered with, system performance degrades drastically, preventing the robot from achieving accurate alignment in complex dynamic environments. Summary of the Invention

[0004] This application provides a robot precise alignment method, system, electronic device, and computer-readable storage medium, aiming to improve the problems of inaccurate alignment and poor robustness of robots in dynamic environments caused by interference from dynamic objects and insufficient sensor fusion depth.

[0005] This application provides a method for precise robot alignment, comprising: acquiring the robot's LiDAR point cloud, visual image, and inertial measurement unit (IMU) data; performing dynamic point cloud segmentation on the LiDAR point cloud, filtering out dynamic interference points to obtain a static point cloud, and extracting a target point cloud from the static point cloud; constructing a tightly coupled fusion model based on the visual image, the target point cloud, and the IMU data, and solving the tightly coupled fusion model to obtain the robot's current pose; calculating the deviation between the current pose and a preset standard pose, and controlling the robot to move according to the deviation to complete the alignment.

[0006] The technical solution provided in this application effectively filters out the pollution of environmental features by dynamic obstacles (such as pedestrians and vehicles) by dynamically segmenting the lidar point cloud before positioning, thus ensuring the purity of the input data from the source. At the same time, by constructing a tightly coupled fusion model of vision, lidar and inertial measurement unit, and using the complementary characteristics of different sensors (such as the distance accuracy of lidar, the texture features of vision, and the high frequency response of IMU) for joint optimization, compared with loosely coupled architecture, the accuracy and robustness of pose estimation can be significantly improved, thereby achieving accurate positioning.

[0007] In one embodiment, the step of performing dynamic point cloud segmentation on the lidar point cloud and filtering out dynamic interference points to obtain a static point cloud includes: constructing a static background point cloud model; calculating the distance between a point in the lidar point cloud of the current frame and the nearest point in the static background point cloud model; if the distance is greater than a preset dynamic threshold, then marking the corresponding point as a dynamic point and filtering it out, and retaining the remaining points as the static point cloud.

[0008] This embodiment constructs a static background model and makes judgments based on distance thresholds, which can quickly and effectively identify newly added or moving objects in the environment, ensuring that subsequent localization algorithms are based only on stable static environmental features, thus improving the adaptability of the algorithm in dynamic scenes.

[0009] In one embodiment, constructing a static background point cloud model includes: accumulating point cloud data from multiple frames of static environment during the system initialization phase, and updating the static background point cloud model online to adapt to environmental changes; extracting target point cloud from the static point cloud includes: processing the static point cloud using a clustering algorithm or a model fitting algorithm to identify the geometric structure data corresponding to the target as the target point cloud.

[0010] This embodiment enables the system to adapt to long-term environmental changes (such as changes in the location of goods stacking) by updating the background model online; and reduces the reliance on manual identification by extracting target point clouds (such as charging pile structures) through clustering or fitting algorithms, thereby improving the system's versatility.

[0011] In one embodiment, the step of constructing a tightly coupled fusion model based on the visual image, the target point cloud, and the inertial measurement unit (IMU) data, and solving the tightly coupled fusion model to obtain the robot's current pose, includes: extracting visual feature points from the visual image; extracting geometric features from the target point cloud; performing pre-integration processing using the IMU data to obtain relative pose change information; constructing the tightly coupled fusion model based on the visual feature points, the geometric features, and the relative pose change information, and optimizing the tightly coupled fusion model using a nonlinear optimization algorithm to obtain the robot's current pose.

[0012] This embodiment extracts visual features, geometric features, and IMU pre-integration information separately, transforming multi-source information into unified mathematical constraints. It then uses a nonlinear optimization algorithm to find the global optimal solution, avoiding the error accumulation problem in traditional filtering methods and significantly improving positioning accuracy.

[0013] In one embodiment, constructing the tightly coupled fusion model based on the visual feature points, the geometric features, and the relative pose change information includes: constructing a laser odometry factor based on the geometric features; constructing a visual reprojection factor based on the visual feature points; constructing an inertial measurement unit (IMU) pre-integration factor based on the relative pose change information; and constructing a factor graph model containing the laser odometry factor, the visual reprojection factor, and the IMU pre-integration factor as the tightly coupled fusion model, wherein the robot pose is the optimization variable of the factor graph model.

[0014] This embodiment uses factor graph optimization, which can flexibly integrate constraints from sensors of different frequencies and types. When a sensor fails (such as visual occlusion), other factors can still maintain the basic operation of the system, thus achieving graceful degradation of the system.

[0015] In one embodiment, the laser odometry factor is obtained by matching the geometric features between two consecutive frames to calculate the pose transformation constraint; the visual reprojection factor is obtained by calculating the matching reprojection error between the visual feature points of the current frame and the image features of the key frame, or by calculating the associated reprojection error between the visual feature points and the geometric features; and the inertial measurement unit pre-integration factor is obtained by using the relative pose change information as the relative motion constraint between adjacent frames.

[0016] This embodiment defines in detail the construction method of each factor, ensuring that the geometric consistency of the laser, the photometric / geometric consistency of the vision, and the kinematic consistency of the IMU are constrained within the same framework.

[0017] In one embodiment, controlling the robot's movement based on the deviation to complete alignment includes: calculating the pose deviation between the current pose and the preset standard pose in the lateral, longitudinal, and angular directions; generating linear velocity control commands and angular velocity control commands based on the pose deviation; and driving the robot chassis to move according to the linear velocity control commands and the angular velocity control commands until the pose deviation is less than a preset threshold.

[0018] This embodiment uses closed-loop control logic to convert high-precision pose estimation results into smooth motion commands, ensuring that the robot can accurately approach and stop at the target position.

[0019] This application embodiment also provides a robot precise alignment system, including: a sensor module for acquiring the robot's LiDAR point cloud, visual image, and inertial measurement unit data; a calculation and processing module for dynamically segmenting the LiDAR point cloud, filtering out dynamic interference points to obtain a static point cloud, and extracting a target point cloud from the static point cloud; and constructing a tightly coupled fusion model based on the visual image, the target point cloud, and the inertial measurement unit data to solve the robot's current pose; and a control execution module for driving the robot to move to complete alignment based on the deviation between the current pose and a preset standard pose.

[0020] This application also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the method described in any embodiment of this application.

[0021] This application also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described in any embodiment of this application.

[0022] As can be seen, in one of the technical solutions provided in this application, by dynamically segmenting the lidar point cloud before positioning, the pollution of environmental features by dynamic obstacles (such as pedestrians and vehicles) is effectively filtered out, ensuring the purity of the input data from the source. At the same time, by constructing a tightly coupled fusion model of vision, lidar and inertial measurement unit, and using the complementary characteristics of different sensors (such as the distance accuracy of lidar, the texture features of vision, and the high frequency response of IMU) for joint optimization, compared with the loosely coupled architecture, the accuracy and robustness of pose estimation can be significantly improved, thereby achieving accurate positioning. Attached Figure Description

[0023] To more clearly illustrate the technical solutions of the embodiments of this application, the drawings used in the description of the embodiments of this application will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0024] Figure 1 This is a flowchart of a robot precise alignment method provided in an embodiment of this application; Figure 2 This is a flowchart illustrating dynamic point cloud segmentation and target extraction in one embodiment of this application; Figure 3 This is a schematic flowchart of a multi-sensor tightly coupled pose estimation provided in an embodiment of this application; Figure 4This is a structural block diagram of the robot precision alignment system provided in the embodiments of this application; Figure 5 This is a structural diagram of an electronic device provided in an embodiment of this application. Detailed Implementation

[0025] To make the technical problems, technical solutions, and beneficial effects solved by this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0026] In existing technologies, robot positioning generally assumes a static environment, relying on LiDAR scanning of pre-set reflectors or QR codes. However, in practical applications, the environment often contains moving objects such as people and forklifts, which can contaminate the LiDAR point cloud data and cause positioning failures. Furthermore, existing solutions often employ loosely coupled architectures, failing to fully leverage the complementary advantages of multiple sensors. To address these issues, this application proposes a solution based on dynamic point cloud segmentation and tight coupling of multiple sensors.

[0027] Terminology Explanation In this application, an inertial measurement unit (IMU) refers to a device used to measure the three-axis attitude angles (or angular rates) and acceleration of an object, typically including a three-axis gyroscope and a three-axis accelerometer.

[0028] In this application, tight coupling refers to fusion at the level of raw sensor data or features. It is usually achieved by jointly optimizing the constraints of all sensors to solve for the state, rather than simply fusing the pose results output independently by each sensor.

[0029] In one embodiment, this application provides a method for precise robot alignment. Please refer to... Figure 1 This includes the following steps: S10: Acquire the robot's LiDAR point cloud, visual images, and inertial measurement unit data; In this step, the system synchronously collects environmental and its own motion data through the sensor module.

[0030] Specifically, LiDAR is used to acquire high-precision 3D point cloud data of the environment. Single-line or multi-line (e.g., 16-line, 32-line) LiDAR can be used, with a typical acquisition frequency of 10Hz or 20Hz. Visual images can be acquired by RGB-D cameras (depth cameras) or standard industrial cameras to obtain color image textures, depth information, and visual features of the environment. Inertial Measurement Unit (IMU) is used to measure the robot's three-axis angular velocity and three-axis acceleration, providing high-frequency (e.g., above 200Hz) motion information.

[0031] As an example of an application scenario, in situations involving high-speed movement or drastic changes in lighting (such as warehouse entrances and exits), traditional cameras are prone to motion blur or overexposure / underexposure. In such cases, the visual image acquisition device can also employ an event camera, leveraging its asynchronous response based on pixel brightness changes and its ultra-high dynamic range to assist the system in operating under extreme lighting conditions.

[0032] In addition, to ensure data consistency, the system will perform hard-triggered synchronization or soft synchronization on the above sensors and pre-calibrate the extrinsic parameter matrices (i.e., rotation and translation relationships) between each sensor so that the data can be unified into the same coordinate system in the future.

[0033] S20: Perform dynamic point cloud segmentation on the lidar point cloud, filter out dynamic interference points to obtain a static point cloud, and extract the target point cloud from the static point cloud; This step aims to separate clean, static environmental data from the raw data and further extract key targets for alignment. In real-world warehouse or factory environments, there are often moving objects such as forklifts and workers. If this data is not processed, it can be mistaken for environmental features (such as walls or pillars), leading to ghosting or incorrect stretching during point cloud registration. Through dynamic point cloud segmentation in this step, the system can identify and remove these dynamic disturbances in real time, retaining only the point cloud data of the background environment. Furthermore, in the clean static point cloud, the system can locate specific alignment targets (such as the geometric outline of a charging pile, the V-shaped support leg of a shelf, etc.) instead of relying on manual markers such as reflectors, thereby improving environmental adaptability.

[0034] S30: Construct a tightly coupled fusion model based on the visual image, the target point cloud, and the inertial measurement unit data, and solve the tightly coupled fusion model to obtain the robot's current pose; This step is the core of this application. The system does not rely on a single sensor, but deeply fuses (tightly couples) visual, laser and IMU data at the feature level.

[0035] In terms of technical principles, loose coupling typically involves each sensor calculating its own pose (e.g., laser and vision sensors each calculate their own), and then averaging these calculations using Kalman filtering. Tight coupling, on the other hand, incorporates the point and line features of the laser sensor, the feature points of the vision sensor, and the integral constraints of the IMU into the same nonlinear optimization function. The advantage of this approach is that when data from a single sensor is insufficient to independently calculate the pose (e.g., vision sensors have limited features and cannot calculate the pose independently), it can still provide partial constraints (such as rotational constraints) to assist other sensors in solving the problem, thereby significantly improving the system's ultimate performance.

[0036] S40: Calculate the deviation between the current pose and the preset standard pose, and control the robot to move according to the deviation to complete the alignment.

[0037] In this step, the high-precision current pose obtained in step S30 is compared with the preset standard pose (such as the standard parking pose of a charging pile) and the deviation value (Δx, Δy, Δθ) is calculated.

[0038] in: Δx (Longitudinal Deviation / Distance Deviation): This typically refers to the difference between the robot's current position and the target position along the X-axis (usually defined as the robot's forward direction or the target's forward / backward direction). It represents how far the robot needs to move forward or backward to reach the target point.

[0039] Δy (lateral deviation / offset): This typically refers to the difference between the robot's current position and the target position along the Y-axis (usually defined as the lateral direction perpendicular to the forward direction). It represents how much the robot has deviated to the left or right relative to the target centerline, requiring lateral movement or path adjustment to eliminate this offset.

[0040] Δθ (Angle Deviation / Heading Angle Deviation): This refers to the angle difference between the robot's current heading and the target's docking orientation. It represents how many degrees the robot needs to rotate to achieve parallelism or alignment with the target when its body is not aligned.

[0041] Subsequently, based on the deviation value, the rotation speed command of the chassis motor is generated through inverse kinematics to drive the robot to move until the deviation meets the preset accuracy requirements.

[0042] This embodiment effectively solves the positioning drift problem caused by dynamic object interference by introducing a dynamic point cloud segmentation step before pose calculation. At the same time, by constructing a multi-sensor tightly coupled fusion model, it overcomes the defect of the existing loosely coupled scheme that the performance drops sharply when a single sensor fails or is interfered with, and significantly improves the positioning accuracy and reliability of the robot in complex dynamic environments.

[0043] In one embodiment, step S20, which involves dynamically segmenting the lidar point cloud to filter out dynamic interference points and obtain a static point cloud, includes steps S21 and S23 (please refer to...). Figure 2 ): S21: Construct a static background point cloud model.

[0044] During the system initialization phase, point cloud data from multiple frames of static environment are accumulated and constructed, and the static background point cloud model is updated online to adapt to environmental changes.

[0045] Specifically, when the robot first operates or is in a known static area, the system collects multiple frames of point cloud data, overlays them, and performs voxel filtering to generate an initial background map. However, the factory environment is not static (e.g., goods are temporarily stacked). Therefore, the system can employ an online, slow update mechanism (e.g., an update strategy based on a probabilistic grid map): if the point cloud of a certain area remains stable for a period of time (e.g., 1 minute), its probability value is increased, gradually integrating it into the background model; conversely, its probability is decreased until it is removed. This mechanism enables the background model to have the ability to remember and forget.

[0046] S22: Calculate the distance between a point in the lidar point cloud of the current frame and the nearest point in the static background point cloud model; S23: If the distance is greater than the preset dynamic threshold, the corresponding point is marked as a dynamic point and filtered out, and the remaining points are retained as the static point cloud.

[0047] Specifically, for each point acquired by the LiDAR in the current frame, its nearest neighbor is found in the background model, and the Euclidean distance is calculated. If the distance exceeds a preset dynamic threshold (e.g., 30cm), it means that the point does not exist in the known background and is very likely a newly added or moving object in the scene (i.e., dynamic interference). Therefore, it is marked as a dynamic point and filtered out.

[0048] As an example, the preset dynamic threshold is not fixed and can be adaptively adjusted according to the sensor's noise level or the complexity of the environment. Besides distance-based methods, as another alternative, deep learning-based methods (such as PointNet++ or RandLA-Net) can be used to classify point clouds point by point, directly outputting static or dynamic labels. This method has stronger semantic understanding capabilities in complex scenes.

[0049] This embodiment provides a computationally efficient and adaptable dynamic point filtering method by constructing a background model with online update capabilities and using distance threshold determination. This method purifies the input data from the source and ensures that the positioning algorithm is not affected by dynamic pedestrian or vehicle traffic.

[0050] In one embodiment, the step S20 of extracting the target point cloud from the static point cloud includes: processing the static point cloud using a clustering algorithm or a model fitting algorithm to identify the geometric structure data corresponding to the target as the target point cloud.

[0051] Based on the static point cloud that has been filtered out of dynamic interference, the system needs to find the specific object where the robot should stop. As an example, if the target is an object of unknown shape, Euclidean clustering or region growing can be used to segment the point cloud into different clusters, and the target can be selected based on the size and aspect ratio of the clusters. If the target has a specific shape (such as a cylindrical charging pile or a flat wall), the RANSAC (Random Sample Consensus) algorithm can be used for model fitting to accurately segment the target point cloud.

[0052] The geometric structure data corresponding to the alignment target refers to the set of point clouds in three-dimensional space that can describe the physical contour of the alignment target. It is usually represented as a cluster of point clouds with specific geometric features.

[0053] For example, when the target is a cylindrical charging pile, the geometric data is represented as a point cloud showing the side profile of the cylinder; when the target is a V-shaped shelf leg, the data is represented as two sets of linear or planar point clouds distributed at an angle; when the target is a wall-to-wall contact platform, the data is represented as a planar point cloud with a specific aspect ratio. This data includes not only the three-dimensional coordinates (x, y, z) of the points, but also geometric parameters fitted from these points (such as the radius and axial direction of the cylinder, the normal vector of the plane, etc.). By identifying this specific geometric data, the system can distinguish the target from complex background environments (such as walls and clutter) and use it as the sole reference for subsequent calculations of relative pose.

[0054] In other words, geometric structure data refers to a set of point clouds in three-dimensional space that can describe the physical contour of the target object. The specific definition is not limited and depends on the shape of the target object in the actual application. As an example, when a robot needs to dock with a cylindrical charging station, the system uses the RANSAC algorithm to search and fit a cluster of cylindrical point clouds within a preset radius range in the static point cloud; this cluster of point clouds constitutes the geometric structure data. As another example, when the target object is a right-angled corner with reflective stripes, the data is represented as two sets of mutually perpendicular planar point clouds or edge line point clouds. By identifying these data with specific geometric features (such as cylindricity, flatness, and specific angles), the system can accurately separate the target object from the background wall or surrounding debris.

[0055] This embodiment extracts target features from a clean, static point cloud, avoiding interference from dynamic clutter in target identification, ensuring the accuracy of subsequent alignment with the reference target, and eliminating the need for manual marking such as reflectors, thus reducing deployment costs and improving the flexibility of on-site deployment.

[0056] In one embodiment, step S30, which involves constructing a tightly coupled fusion model based on the visual image, the target point cloud, and the inertial measurement unit data, and solving the tightly coupled fusion model to obtain the robot's current pose, includes steps S31-S34 (please refer to...). Figure 3 ): S31: Extract visual feature points from the visual image.

[0057] For example, keypoints can be extracted from RGB images using feature extraction algorithms (such as ORB, SIFT, SURF, etc.) and their descriptors can be calculated. ORB (Oriented Fast and Rotated BRIEF) features are preferred due to their fast computation speed, making them suitable for real-time systems. After feature extraction, feature matching or optical flow tracing is typically performed to establish inter-frame relationships.

[0058] S32: Extract geometric features from the target point cloud.

[0059] For example, based on curvature calculation, edge features (points with high curvature, such as corners of walls and edges of pillars) and planar features (points with low curvature, such as the ground and walls) are extracted from the target point cloud.

[0060] S33: Use the data from the inertial measurement unit to perform pre-integration processing to obtain relative pose change information.

[0061] Since the data frequency of the IMU (typically above 200Hz) is much higher than that of the LiDAR and camera, directly integrating the IMU data would result in excessive computation due to frequent updates of state variables. This embodiment employs a pre-integration technique, integrating the IMU measurements (angular velocity and acceleration) in the body coordinate system between two adjacent laser or image keyframes to form an independent relative motion constraint. This constraint depends only on the IMU measurements and the zero bias, and is independent of the initial state of the previous frame. This avoids re-integrating the IMU data every time the state is adjusted during optimization, significantly reducing the computational load.

[0062] S34: Construct the tightly coupled fusion model based on the visual feature points, the geometric features, and the relative pose change information, and use a nonlinear optimization algorithm to optimize and solve the tightly coupled fusion model to obtain the robot's current pose.

[0063] The nonlinear optimization algorithm can be the Levenberg-Marquardt (LM) algorithm, the Gauss-Newton (GN) algorithm, or the Dog-Leg algorithm, etc.

[0064] This embodiment incorporates environmental information from different dimensions into a unified computational framework by extracting visual, geometric, and inertial features separately. Pre-integration processing not only solves the synchronization and alignment problem between high-frequency IMU data and low-frequency visual / laser data but also ensures real-time performance. Through nonlinear optimization, the optimal pose solution can be found while satisfying all sensor constraints, thereby minimizing errors.

[0065] In one embodiment, the construction of the tightly coupled fusion model in step S34 specifically includes: constructing a laser odometry factor based on the geometric features; constructing a visual reprojection factor based on the visual feature points; constructing an inertial measurement unit pre-integration factor based on the relative pose change information; and constructing a factor graph model containing the laser odometry factor, the visual reprojection factor, and the inertial measurement unit pre-integration factor as the tightly coupled fusion model, wherein the robot pose is the optimization variable of the factor graph model.

[0066] Specifically, the tightly coupled fusion model is embodied in a factor graph model. A factor graph is a probabilistic graphical model in which nodes represent state variables to be optimized (such as pose, velocity, and IMU bias), and edges represent factors (constraints).

[0067] (1) The laser odometry factor is obtained by matching the geometric features between two consecutive frames (such as using the ICP algorithm or NDT algorithm) to calculate the pose transformation constraint, which constrains the relative motion of the robot. Laser odometry (LAO) is a relative motion constraint used to constrain the pose change of a robot between two consecutive moments (two frames). It calculates the distance and angle the robot has moved during this time by comparing the geometric differences between the laser point clouds of the two frames. For example, geometric features, such as edge features (points with high curvature, like the edge of a pillar) and planar features (points with low curvature, like walls or the ground), are first extracted from the static point clouds of the current and previous frames. Point cloud registration algorithms (such as ICP Iterative Closest Point (ICP) or NDT Normal Distribution Transform (NDT) algorithms) are then used to match the feature points of the current frame with those of the previous frame (or the local map). By minimizing the distance error between the matched points, the pose transformation matrix (including rotation and translation) between the two frames is calculated. This transformation matrix constitutes the LAO in the factor graph, connecting adjacent pose nodes. (2) The visual reprojection factor is obtained by calculating the matching reprojection error between the visual feature points of the current frame and the keyframe image features (i.e., the pixel distance between the observed position of the feature point on the image and the projected position predicted according to the pose). This factor plays a major role in scenarios with rich textures but simple structures (such as only a white wall with a painting hanging on it). The visual reprojection factor is a constraint based on visual geometric consistency. It constrains the error between the observed image feature point positions and the pixel positions calculated based on the current estimated pose projection. Simply put, it ensures that the robot's pose estimation allows the camera to reasonably locate the feature points. For example, visual feature points (such as ORB features) are extracted from the current frame image and matched with feature points in keyframes, or associated with known 3D spatial points (landmarks). Based on the currently estimated robot pose and the camera's intrinsic model, the 3D spatial points (or depth backprojections of feature points from the previous frame) are projected onto the image plane of the current frame, resulting in predicted pixel coordinates. The distance between the actually observed pixel coordinates and the predicted pixel coordinates (i.e., the reprojection error) is calculated. This error is constructed as the visual reprojection factor, and the optimization process aims to minimize this error.

[0068] (3) The pre-integration factor of the inertial measurement unit is obtained by using the relative pose change information as a relative motion constraint between adjacent frames. In the instant when a violent movement or slippage causes visual / laser failure, this factor can maintain the pose estimation by inertial calculation.

[0069] The IMU pre-integration factor is a high-frequency motion constraint. Because the sampling frequency of the IMU (e.g., 200Hz) is much higher than that of the laser and camera (e.g., 10Hz), direct integration is computationally intensive and dependent on the initial state. The pre-integration factor pre-integrates all IMU measurements over a period of time between two image / laser frames into an independent relative motion increment, which is then added as a constraint to the optimization.

[0070] Between two keyframes (e.g., frame i and frame j), a series of raw IMU data (angular velocity and acceleration) are acquired. In the robot's body coordinate system, the acceleration and angular velocity during this time are integrated to calculate the relative position change, relative velocity change, and relative rotation change between the two frames. This process compresses this string of high-frequency data into a constraint. The calculated relative changes are then used to construct a pre-integration factor. This factor is only related to the IMU measurements and bias, and is decoupled from the robot's absolute pose. Therefore, when optimizing and adjusting the pose, it is not necessary to re-integrate the IMU data, greatly improving computational efficiency.

[0071] This embodiment utilizes factor graphs as a tightly coupled mathematical carrier to achieve complementary advantages of multi-source information. The factor graph architecture exhibits strong robustness and graceful degradation capabilities: for example, in long corridor scenarios (geometric degradation), laser constraints weaken, and the system automatically relies more on visual textures and IMU constraints; in dark scenarios, visual constraints fail, and the system automatically relies on lasers and IMUs. This adaptive weight adjustment mechanism ensures the robot's alignment stability in complex dynamic environments.

[0072] In one embodiment, step S40 includes S41-S43: S41: Calculate the pose deviation between the current pose and the preset standard pose in the horizontal, vertical and angular directions.

[0073] This involves calculating Δx, Δy, and Δθ. This calculation typically needs to be performed in the target coordinate system to convert the global pose difference into the robot's local error.

[0074] S42: Generate linear velocity control commands and angular velocity control commands based on the pose deviation.

[0075] As an example, three independent PID controllers can be used to control the longitudinal, lateral, and angular errors respectively, which is a simple and easy method. Alternatively, a more advanced model predictive control (MPC) algorithm can be used to predict a future trajectory and generate the optimal control law at the current moment, while satisfying the robot's kinematic constraints (such as maximum speed, maximum acceleration, and maximum acceleration / deceleration). The MPC method can make the robot's motion smoother and reduce overshoot and oscillation.

[0076] S43: Drive the robot chassis to move according to the linear velocity control command and the angular velocity control command until the pose deviation is less than a preset threshold.

[0077] For example, the distance error is <5mm and the angle error is <0.5 degrees.

[0078] This embodiment uses a closed-loop feedback control mechanism to convert the high-precision pose output by the front-end sensing module into motion control quantities for the chassis in real time. The introduction of advanced control algorithms such as MPC enables the robot to maintain stability even at low speeds when approaching the target, avoiding nodding or swaying, and ultimately achieving high-precision positioning and docking.

[0079] This application also provides a robot precision alignment system. Please refer to... Figure 4 ,include: Sensor module 410 is used to acquire lidar point clouds, visual images, and inertial measurement unit data for the robot. This module can be integrated into the top or front of the robot body.

[0080] The computational processing module 420 is used to perform dynamic point cloud segmentation on the lidar point cloud, filter out dynamic interference points to obtain a static point cloud, and extract the target point cloud from the static point cloud; and to construct a tightly coupled fusion model based on the visual image, the target point cloud and the inertial measurement unit data to solve the robot's current pose.

[0081] The control execution module 430 is used to drive the robot to move to complete the alignment based on the deviation between the current pose and the preset standard pose. This module typically includes a motion control algorithm and a chassis motor driver.

[0082] In one embodiment, the calculation processing module 420 is further specifically used for: Construct a static background point cloud model; Calculate the distance between a point in the current frame's lidar point cloud and the nearest point in the static background point cloud model; If the distance is greater than a preset dynamic threshold, the corresponding point is marked as a dynamic point and filtered out, and the remaining points are retained as the static point cloud.

[0083] In one embodiment, the calculation processing module 420 is further specifically used for: During the system initialization phase, point cloud data from multiple frames of static environment are accumulated and constructed, and the static background point cloud model is updated online to adapt to environmental changes. The step of extracting the target point cloud from the static point cloud includes: processing the static point cloud using a clustering algorithm or a model fitting algorithm to identify the geometric structure data corresponding to the target as the target point cloud.

[0084] In one embodiment, the calculation processing module 420 is further specifically used for: Extract visual feature points from the visual image; Extract geometric features from the target point cloud; The relative pose change information is obtained by pre-integrating the data from the inertial measurement unit. The tightly coupled fusion model is constructed based on the visual feature points, the geometric features, and the relative pose change information. The tightly coupled fusion model is then optimized and solved using a nonlinear optimization algorithm to obtain the robot's current pose.

[0085] In one embodiment, the calculation processing module 420 is further specifically used for: A laser odometry factor is constructed based on the aforementioned geometric features; A visual reprojection factor is constructed based on the aforementioned visual feature points; Based on the relative pose change information, a pre-integration factor for the inertial measurement unit is constructed; A factor graph model is constructed, which includes the laser odometry factor, the visual reprojection factor, and the inertial measurement unit pre-integration factor, as the tightly coupled fusion model, wherein the robot pose is the optimization variable of the factor graph model.

[0086] In one embodiment, the laser odometry factor is obtained by matching the geometric features between two consecutive frames to calculate the pose transformation constraint; the visual reprojection factor is obtained by calculating the matching reprojection error between the visual feature points of the current frame and the image features of the key frame, or by calculating the associated reprojection error between the visual feature points and the geometric features; the inertial measurement unit pre-integration factor is obtained by using the relative pose change information as the relative motion constraint between adjacent frames.

[0087] In one embodiment, the control execution module 430 is specifically used for: Calculate the pose deviations between the current pose and the preset standard pose in the horizontal, vertical, and angular directions; Based on the pose deviation, linear velocity control commands and angular velocity control commands are generated; The robot chassis is driven to move according to the linear velocity control command and the angular velocity control command until the pose deviation is less than a preset threshold.

[0088] Regarding the robot precise alignment system provided in the embodiments of this application, the corresponding description of the robot precise alignment method described above can be used, and will not be repeated here.

[0089] This application also provides an electronic device, please refer to... Figure 5 The device includes a memory and a processor. The memory stores computer programs, and the processor executes the programs stored in the memory to implement the precise robot alignment method described in any embodiment of this application. This electronic device can be a vehicle-mounted industrial control computer or an embedded computing platform for the robot; specific embodiments of this application are not limited to this.

[0090] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the robot precise alignment method described in any embodiment of this application.

[0091] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A method for precise robot alignment, characterized in that, include: Acquire the robot's LiDAR point cloud, visual images, and inertial measurement unit data; The lidar point cloud is dynamically segmented to filter out dynamic interference points and obtain a static point cloud, and the target point cloud is extracted from the static point cloud. A tightly coupled fusion model is constructed based on the visual image, the target point cloud, and the inertial measurement unit data, and the tightly coupled fusion model is solved to obtain the robot's current pose; The deviation between the current pose and the preset standard pose is calculated, and the robot is controlled to move according to the deviation to complete the alignment.

2. The method according to claim 1, characterized in that, The step of performing dynamic point cloud segmentation on the lidar point cloud and filtering out dynamic interference points to obtain a static point cloud includes: Construct a static background point cloud model; Calculate the distance between a point in the current frame's lidar point cloud and the nearest point in the static background point cloud model; If the distance is greater than a preset dynamic threshold, the corresponding point is marked as a dynamic point and filtered out, and the remaining points are retained as the static point cloud.

3. The method according to claim 2, characterized in that, The construction of the static background point cloud model includes: During the system initialization phase, point cloud data from multiple frames of static environment are accumulated and constructed, and the static background point cloud model is updated online to adapt to environmental changes. The step of extracting the target point cloud from the static point cloud includes: processing the static point cloud using a clustering algorithm or a model fitting algorithm to identify the geometric structure data corresponding to the target as the target point cloud.

4. The method according to claim 1, characterized in that, The process of constructing a tightly coupled fusion model based on the visual image, the target point cloud, and the inertial measurement unit data, and solving the tightly coupled fusion model to obtain the robot's current pose, includes: Extract visual feature points from the visual image; Extract geometric features from the target point cloud; The relative pose change information is obtained by pre-integrating the data from the inertial measurement unit. The tightly coupled fusion model is constructed based on the visual feature points, the geometric features, and the relative pose change information. The tightly coupled fusion model is then optimized and solved using a nonlinear optimization algorithm to obtain the robot's current pose.

5. The method according to claim 4, characterized in that, The construction of the tightly coupled fusion model based on the visual feature points, the geometric features, and the relative pose change information includes: A laser odometry factor is constructed based on the aforementioned geometric features; A visual reprojection factor is constructed based on the aforementioned visual feature points; Based on the relative pose change information, a pre-integration factor for the inertial measurement unit is constructed; A factor graph model is constructed, which includes the laser odometry factor, the visual reprojection factor, and the inertial measurement unit pre-integration factor, as the tightly coupled fusion model, wherein the robot pose is the optimization variable of the factor graph model.

6. The method according to claim 5, characterized in that: The laser odometry factor is obtained by matching the geometric features between two consecutive frames to calculate the pose transformation constraint. The visual reprojection factor is obtained by calculating the matching reprojection error between the visual feature points of the current frame and the key frame image features, or by calculating the correlation reprojection error between the visual feature points and the geometric features. The pre-integration factor of the inertial measurement unit is obtained by using the relative pose change information as a relative motion constraint between adjacent frames.

7. The method according to any one of claims 1-6, characterized in that, The step of controlling the robot's movement based on the deviation to complete the alignment includes: Calculate the pose deviations between the current pose and the preset standard pose in the horizontal, vertical, and angular directions; Based on the pose deviation, linear velocity control commands and angular velocity control commands are generated; The robot chassis is driven to move according to the linear velocity control command and the angular velocity control command until the pose deviation is less than a preset threshold.

8. A robot precision alignment system, characterized in that, include: The sensor module is used to acquire the robot's LiDAR point cloud, visual images, and inertial measurement unit data; The computational processing module is used to perform dynamic point cloud segmentation on the lidar point cloud, filter out dynamic interference points to obtain a static point cloud, and extract the target point cloud from the static point cloud; and to construct a tightly coupled fusion model based on the visual image, the target point cloud, and the inertial measurement unit data to solve the robot's current pose. The control execution module is used to drive the robot to move to complete the alignment based on the deviation between the current pose and the preset standard pose.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the method as described in any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1 to 7.

Images