Mobile robot accurate docking method based on edge calculation

Abstract

The invention discloses a mobile robot accurate docking method based on edge calculation, and aims to solve the problems that the dynamic docking precision is reduced and the collision risk is increased due to micro-motion or drifting of a target station. According to the method, unified time reference alignment is carried out on data of a camera, a laser radar, an inertial measurement unit, an ultra-wideband range finder, a station encoder and a programmable logic controller at an edge node, and a three-dimensional special Euclidean group equivariant multi-source fusion network is used for outputting relative pose estimation and covariance; the estimation in the time window is further used as a condition to be input into a conditional diffusion short-time prediction model to obtain a time-varying mean value and a time-varying covariance, an anisotropic probability tube is constructed, and prediction-measurement joint correction is carried out based on a score function; scenarized opportunity constraints are constructed under correction probability tube constraints, a tubular nonlinear model is adopted to predict, control and solve a reference trajectory, an actuator command is generated in combination with depth visual servo and compliance control, and the technical effects of high precision, robustness and safe docking under the station dynamic disturbance condition are achieved.

Description

Technical Field

[0001] This invention relates to the field of mobile robots, and more particularly to a precise docking method for mobile robots based on edge computing. Background Technology

[0002] Mobile robots are widely used in smart manufacturing and warehousing logistics for material handling, loading and unloading, and automatic charging. Precise docking between the mobile robot and the target workstation is a crucial foundation for achieving unmanned operations. Current technologies typically employ methods such as camera recognition of artificial markers or natural features, lidar for contour matching or reflection feature localization, and environmental map-based localization. To improve positioning stability, multi-sensor fusion of information from wheel encoders, inertial measurement units, and ultra-wideband ranging is often used. For docking control, existing solutions often employ trajectory following control or constraint optimization control based on model predictive control, and introduce visual servoing and impedance control or compliant control during the end-effector contact or insertion stage to improve docking success rate and reduce contact impact. With the development of edge computing, some solutions are beginning to deploy perception fusion and control computation at edge nodes to reduce reliance on the cloud and improve real-time performance.

[0003] The existing technology still has the following shortcomings:

[0004] 1. Most docking solutions assume that the target workstation is stationary or has only negligible disturbances. They lack short-term prediction mechanisms for dynamic changes such as workstation micro-movements and drifts, as well as their uncertainty expressions, which can easily lead to increased docking errors, insertion failures, or increased collision risks.

[0005] 2. Under industrial wireless network conditions, multi-source sensor data and station-side signals are prone to clock bias, link delay and sampling asynchrony. If there is a lack of a unified time reference and reliable time alignment processing, fusion estimation is prone to systematic bias and weakens the credibility of uncertainty assessment.

[0006] 3. Existing robust control and stochastic control often employ fixed safety margins or simplified noise assumptions, making it difficult to characterize pose uncertainties that vary over time and have directional differences. This results in control strategies that are either too conservative, affecting efficiency, or insufficiently constrained, affecting safety and stability.

[0007] Therefore, a precise docking method for mobile robots that can overcome the shortcomings of the existing technology is a problem that needs to be solved by those skilled in the art. Summary of the Invention

[0008] One objective of this invention is to propose a precise docking method for mobile robots based on edge computing. Addressing the shortcomings of existing technologies, such as lack of short-time prediction and uncertainty representation when there is micro-motion or drift at the target workstation, decreased fusion accuracy due to clock bias and link delays in multi-source sensors and workstation-side signals, and the difficulty in balancing real-time performance and safety constraints with robust control, this invention proposes establishing a unified time reference at the edge nodes and aligning the data from cameras, LiDAR, inertial measurement units, ultra-wideband ranging, workstation encoders, and programmable logic controllers. The aligned multi-source observations are input into a three-dimensional special Euclidean group isovariant multi-source fusion network to obtain relative pose estimation and its covariance. Furthermore, based on a conditional diffusion short-time prediction model, the time-varying mean and covariance in the prediction time domain are output, and an anisotropic probability tube is constructed. Combined with a scoring function, prediction and measurement are jointly corrected. Under the constraints of the corrected probability tube, scenario-based opportunistic constraints are constructed, and a tubular nonlinear model is used for predictive control to solve for the reference trajectory. This is combined with depth vision servoing and compliant control to generate actuator commands. This invention has the technical effect of improving docking accuracy and success rate, reducing collision risk, and enhancing the robustness and real-time performance of the docking process under dynamic disturbance conditions at the workstation.

[0009] This invention provides a method for precise docking of mobile robots based on edge computing, comprising:

[0010] S1. Receive raw data from the camera, lidar, inertial measurement unit, ultra-wideband ranging module of the mobile robot, and encoder and programmable logic controller of the target station. Perform time alignment based on a unified time reference to obtain time-aligned visual observation, lidar observation, inertial observation, ultra-wideband observation, encoder reading and programmable logic controller status.

[0011] S2. Using visual observation, laser observation, inertial observation, and ultra-wideband observation as features, and encoder readings and programmable logic controller states as constraints, input a three-dimensional special Euclidean group and other variable multi-source fusion network to obtain the relative pose estimation and pose covariance of the current control cycle.

[0012] S3. In the most recent time window, the relative pose estimation and pose covariance are spliced ​​together, and the encoder reading and programmable logic controller state are normalized and used as conditions to input the conditional diffusion short-time prediction model to obtain the time-varying mean and time-varying covariance in the prediction time domain. The covariance ellipsoids of each prediction time are connected along the time axis to construct an anisotropic probability tube.

[0013] S4. Using the anisotropic probability tube as a priori and the relative pose estimation and pose covariance of the current control cycle as measurements, the time-varying mean and time-varying covariance are jointly corrected based on the scoring function, and the corrected anisotropic probability tube is reconstructed accordingly.

[0014] S5. Based on the corrected anisotropic probability tube as the constraint, multiple probability scenarios are extracted according to the preset constraint violation probability threshold, scenario-based opportunity constraints are established, and online rolling solution is obtained by using the tube nonlinear model predictive control to obtain the relative velocity reference trajectory and attitude reference trajectory.

[0015] S6. Using the relative velocity reference trajectory and attitude reference trajectory as servo targets, perform feature alignment of time-aligned visual observations within the current control cycle, and solve the end pose increment command by minimizing the feature error.

[0016] S7. Input the end-effector pose increment command into the compliant controller. After gain limiting and force limiting processing, generate actuator control commands and send them to the mobile robot to perform docking according to the fixed control cycle.

[0017] Optionally, S1 includes:

[0018] The edge nodes synchronize their clocks with the plant's time source via a precision time protocol to establish a unified time base. When the precision time protocol is unavailable, the edge nodes' hardware clocks are used as the unified time base.

[0019] The edge node receives raw data from the mobile robot's camera, LiDAR, inertial measurement unit, ultra-wideband ranging module, as well as from the encoder and programmable logic controller at the target workstation, and records the local timestamp of each channel.

[0020] For the local timestamp of each channel, the time offset and frequency deviation between the local time and the unified time reference are estimated based on the sliding window linear regression, and the time offset is compensated by combining the round-trip delay measurement of the wireless link, so as to obtain the time mapping function from the local time to the unified time reference.

[0021] The time mapping function is used to map the timestamps of each channel to a unified time reference. At the boundary moments of a fixed control cycle, each channel is resampled and aligned. The camera obtains time-aligned visual observation by selecting the image frame closest to the boundary moment and performing rolling shutter time correction. The lidar performs motion distortion removal processing on the point cloud within a scanning cycle based on the timestamp and outputs the point cloud of that cycle at the boundary moment to obtain time-aligned lidar observation. The inertial measurement unit integrates or averages the acceleration and angular velocity within the current control cycle to obtain time-aligned inertial observation. The ultra-wideband ranging module selects the ranging value or performs time interpolation near the boundary moment to obtain time-aligned ultra-wideband observation. The encoder and programmable logic controller use the nearest neighbor hold strategy at the boundary moment to obtain time-aligned encoder readings and time-aligned programmable logic controller states, respectively.

[0022] Output time-aligned visual observations, time-aligned laser observations, time-aligned inertial observations, time-aligned ultrawideband observations, time-aligned encoder readings, and time-aligned programmable logic controller states.

[0023] Terminology definition:

[0024] The camera is an imaging sensor that acquires image data of the docking scene and outputs image frames and corresponding timestamps;

[0025] The lidar is a ranging sensor that acquires point cloud data of the docking scene and outputs the point cloud and corresponding timestamps.

[0026] The inertial measurement unit is an inertial sensor module that outputs acceleration and angular velocity data and has a timestamp;

[0027] The ultra-wideband ranging module is a ranging module that outputs ultra-wideband signals and measures the distance between the robot and a preset base station or tag, and has a timestamp.

[0028] The encoder is a sensor used to characterize the position or displacement changes of moving parts at the target workstation and output encoder signals;

[0029] The programmable logic controller is an industrial controller used to output the target workstation's operating status, action stage, or enable signal.

[0030] The time mapping function is a mapping relationship used to map the local timestamps of each channel to a unified time reference time axis;

[0031] The visual observations are time-aligned image data corresponding to the boundary moments and their related measurement information.

[0032] The laser observations consist of point cloud data and related measurement information that have been time-aligned and correspond to the boundary time.

[0033] The inertial observations are time-aligned inertial measurement information corresponding to the boundary times;

[0034] The ultra-wideband observation is the ranging measurement information corresponding to the boundary time after time alignment;

[0035] The encoder reading is the encoder output value corresponding to the boundary moment after time alignment;

[0036] The programmable logic controller (PLC) status is the workstation operating status or control status information corresponding to the boundary time after time alignment.

[0037] Optionally, S2 includes:

[0038] At the edge nodes, time-aligned visual observations, time-aligned laser observations, time-aligned inertial observations, and time-aligned ultrawideband observations are used as feature extraction inputs, and time-aligned encoder readings and time-aligned programmable logic controller states are used as constraint inputs, which are then input into a three-dimensional special Euclidean group and other variable multi-source fusion network.

[0039] The three-dimensional special Euclidean group variable multi-source fusion network includes a static geometric branch and a dynamic cue branch. The static geometric branch extracts geometric features from time-aligned visual observations and time-aligned laser observations, while the dynamic cue branch extracts motion and distance features from time-aligned inertial observations and time-aligned ultra-wideband observations. The two branches are then fused using feature weighting based on observation quality weights. These observation quality weights are calculated based on image sharpness and motion blur indices, laser echo intensity and point cloud consistency indices, and ultra-wideband non-line-of-sight scores.

[0040] The three-dimensional special Euclidean group equal variable multi-source fusion network applies time-aligned encoder readings as relative kinematic constraints and time-aligned programmable logic controller states as workstation state constraints during the fusion phase. It also performs micro-correction on the extrinsic parameter bias across sensors to correct the feature correspondence.

[0041] After completing the above fusion, the relative pose estimate of the current control cycle and the pose covariance corresponding to the relative pose estimate are output.

[0042] Terminology definition:

[0043] The three-dimensional special Euclidean group is a mathematical group structure used to characterize the pose transformation of a rigid body in three-dimensional space, and its elements include three-dimensional translation and three-dimensional rotation.

[0044] The three-dimensional special Euclidean group equivariant multi-source fusion network is a neural network that extracts and fuses features from multiple sources and outputs a relative pose estimate. The equivariant means that when the input observations undergo a consistent change under three-dimensional rigid body transformation, the pose estimate output by the network maintains a consistent transformation relationship.

[0045] The feature is an intermediate representation used to characterize scene geometry and motion information, obtained by network encoding from time-aligned visual observations, time-aligned laser observations, time-aligned inertial observations, and time-aligned ultra-wideband observations.

[0046] The constraints are information inputs used to impose prior or consistency constraints on the fusion inference process, including relative kinematic consistency represented by time-aligned encoder readings and station state consistency represented by time-aligned programmable logic controller states.

[0047] The current control period is a time interval divided by a fixed control period, used to complete one state estimation and control calculation;

[0048] The relative pose estimation is the result of estimating the pose relationship between the mobile robot and the target workstation, and the pose relationship includes at least relative translation and relative rotation;

[0049] The pose covariance is a covariance matrix used to characterize the uncertainty of the relative pose estimation, and its elements reflect the statistical characteristics of the estimation error of each dimension of translation and rotation and their correlation.

[0050] The static geometric branch is a network branch in the three-dimensional special Euclidean group equivariant multi-source fusion network used to extract the environmental structure and geometric correspondence features from time-aligned visual observations and time-aligned laser observations.

[0051] The dynamic cue branch is a network branch in the three-dimensional special Euclidean group equivariant multi-source fusion network used to extract motion state and distance constraint features from time-aligned inertial observations and time-aligned ultrawideband observations.

[0052] The observation quality weight is a weight parameter used to characterize the reliability of different observation channels within the current control period and to adjust the feature fusion contribution of each channel accordingly.

[0053] The feature weighted fusion is a process of weighting and combining features from different branches or different sensor channels according to the observation quality weights to form fused features;

[0054] The non-line-of-sight score is an evaluation metric used to characterize the degree to which the ultra-wideband ranging signal is in a non-line-of-sight propagation state and to evaluate the reliability of ultra-wideband ranging.

[0055] The relative kinematic constraints are relative displacements, relative velocities, or motion consistency relationships characterized by encoder readings, and are used to limit the feasible range of the relative pose estimation.

[0056] The workstation state constraint is the target workstation action stage, enable state, or working mode information represented by the programmable logic controller state, and is used to limit the consistency between the relative pose estimation and the actual workstation state.

[0057] The cross-sensor extrinsic bias refers to the deviation of the calibrated extrinsic parameters relative to the true extrinsic parameters between different sensor coordinate systems, and the extrinsic parameters include at least pose extrinsic parameters.

[0058] The micro-correction refers to the process of making small-scale online corrections to the extrinsic parameter bias during the fusion estimation process to improve the cross-sensor feature correspondence.

[0059] Optionally, S3 includes:

[0060] At the edge node, the relative pose estimation and pose covariance are concatenated in chronological order within the nearest time window of a fixed control cycle. The time-aligned encoder readings and time-aligned programmable logic controller states are then dimensionally consistent and range-normalized and combined with the concatenation result to form the conditional input.

[0061] The conditions are input into the conditional diffusion short-time prediction model. The time-varying mean and time-varying covariance of the relative pose are calculated in the prediction time domain through forward diffusion and reverse denoising sampling. The number of steps and noise intensity of the reverse denoising are set according to a fixed control period to meet the real-time requirements.

[0062] Based on the time-varying mean and time-varying covariance at each prediction time, a spatiotemporal channel is constructed with the time-varying mean as the center and the covariance ellipsoid determined by the time-varying covariance as the cross section, and the covariance ellipsoid is connected along the time axis to obtain an anisotropic probability tube.

[0063] Terminology definition:

[0064] The most recent time window is a historical time interval that traces back from the current control cycle and includes a preset number of control cycles or a preset duration.

[0065] The splicing is a data organization method that combines the relative pose estimates and pose covariances corresponding to each control cycle within the most recent time window in chronological order to form a time-series input.

[0066] The dimension unification refers to the process of scaling or unifying the units of input data for different physical quantities to make them comparable on the same numerical scale.

[0067] The range normalization is a process of mapping the input data to a standard numerical range according to a preset range in order to facilitate stable inference of the model.

[0068] The conditional input is the joint input data to the conditional diffusion short-time prediction model, which includes at least the splicing result and the encoder readings and programmable logic controller states after dimensional unification and range normalization.

[0069] The conditional diffusion short-term prediction model is a generative prediction model that uses the conditional input as a conditional variable to predict the uncertainty distribution of relative pose in the future short-term prediction time domain.

[0070] The prediction time domain is a time interval with a preset prediction length that extends from the current control cycle into the future.

[0071] The forward diffusion refers to the gradual injection of noise into the target variable in the training or inference framework to form a diffusion process from low noise to high noise.

[0072] The reverse denoising sampling is a sampling process in which noise is gradually removed from a high-noise state in the diffusion model and a sample sequence is generated to obtain the predicted distribution parameters.

[0073] The time-varying mean is the mean parameter of the relative pose distribution corresponding to each prediction time in the prediction time domain, and the mean parameter changes with the prediction time.

[0074] The time-varying covariance is the uncertainty covariance parameter of the relative pose distribution corresponding to each prediction time in the prediction time domain, and the covariance parameter changes with the prediction time.

[0075] The covariance ellipsoid is an ellipsoid or ellipsoid boundary determined by the time-varying covariance at a certain prediction time, used to represent the spatial range of relative pose uncertainty at that prediction time.

[0076] The anisotropic probability tube is a spatiotemporal uncertainty channel formed by connecting the covariance ellipsoids of each prediction time along the time axis with the time-varying mean of each prediction time in the prediction time domain as the center line. Anisotropy means that the scale of uncertainty in different directions is determined by the principal axis scale of the covariance ellipsoid and can be different.

[0077] The noise intensity is a parameter used to control the noise injection amplitude during the forward diffusion and / or reverse denoising sampling process;

[0078] The number of steps in the reverse denoising is the number of iterations or the number of offset steps when performing the reverse denoising sampling, which is used to balance real-time performance and prediction accuracy.

[0079] The real-time requirement is the time constraint that must be met to complete the calculation of step S3 and output the prediction result within the fixed control period.

[0080] Optionally, S4 includes:

[0081] At the edge node, the anisotropic probability tube is used as the prior, and the relative pose estimate and its pose covariance of the current control cycle are used as the measurement. The prior is updated once based on the joint correction of prediction and measurement based on the score function.

[0082] The joint correction of prediction and measurement based on the scoring function includes: calculating the gradient of the prior log probability in the prediction time domain corresponding to the prior using the conditional diffusion short-time prediction model as the scoring function; constructing an equivalent gain matrix based on the measurement residual and the pose covariance of the measurement and the time-varying covariance of the prior; performing a weighted update on the time-varying mean of the prior and a shrinkage update on the time-varying covariance of the prior, while keeping the time index of the prediction time domain unchanged.

[0083] Based on the updated time-varying mean and time-varying covariance, the covariance ellipsoid is reconstructed at each prediction time and connected along the time axis to obtain the corrected anisotropic probability tube.

[0084] Terminology definition:

[0085] The prior is used to characterize the relative pose probability distribution information within the current control cycle to the prediction time domain before the introduction of the current measurement information, wherein the prior is determined by the time-varying mean and time-varying covariance corresponding to the anisotropic probability tube;

[0086] The measurement is the relative pose estimate output by the three-dimensional special Euclidean group equal variable multi-source fusion network in the current control cycle and the pose covariance corresponding to the relative pose estimate;

[0087] The joint correction of prediction and measurement is a process of fusing the prior and the measurement under the same uncertainty expression framework to update the prediction distribution parameters;

[0088] The scoring function is the gradient of the prior log probability with respect to the relative pose variable, used to indicate the direction of increasing the prior probability density;

[0089] The measurement residual is the difference between the relative pose estimate corresponding to the measurement and the time-varying mean of the prior at the same time index;

[0090] The equivalent gain matrix is ​​a weighted matrix constructed based on the measurement residual, the measurement pose covariance, and the prior time-varying covariance, and is used to determine the weight allocation of measurement information to the prior update.

[0091] The weighted update is a process of using the equivalent gain matrix to correct the prior time-varying mean to obtain the updated time-varying mean.

[0092] The shrinkage update is a process of reducing or conservatively compressing the prior time-varying covariance after introducing measurement information to obtain the updated time-varying covariance.

[0093] The prediction time index is the sequence number or time identifier of each prediction moment within the prediction time domain.

[0094] The phrase "keeping the time index of the prediction time domain unchanged" means that when performing the joint correction, only the time-varying mean and time-varying covariance corresponding to each prediction time index are updated, without changing the value of each prediction time index and its corresponding prediction time.

[0095] The aforementioned joint correction is a processing method in which a single parameter update is performed on the prior in each control cycle to obtain the correction result;

[0096] The reconstruction involves regenerating the covariance ellipsoid at each prediction time based on the updated time-varying mean and time-varying covariance, and connecting them along the time axis to form a corrected anisotropic probability tube.

[0097] Optionally, S5 includes:

[0098] At the edge nodes, a discrete-time nonlinear relative kinematics model is used as the controlled object, and the corrected anisotropic probability tube is used to determine the set of disturbances at each time point in the prediction time domain.

[0099] Based on the preset constraint violation probability threshold, the covariance ellipsoid corresponding to each prediction time of the corrected anisotropic probability tube is sampled in layers along the principal axis to generate several probability scenarios. The probability scenarios at each prediction time are obtained by superimposing the time-varying mean and the perturbation vector of the covariance ellipsoid at that time.

[0100] Using the relative velocity sequence and attitude sequence in the prediction time domain as decision variables, a rolling optimization problem is constructed. The cost function of the rolling optimization problem includes a target tracking term for relative pose, a velocity matching term, a control increment penalty term, and a terminal stabilization term. The constraints of the rolling optimization problem include state constraints and input constraints that are simultaneously valid for all probability scenarios. The state constraints include at least a minimum approach distance, an attitude deviation, and a relative velocity upper limit. The input constraints include at least a bound on the rate of change of relative velocity and a bound on the rate of change of attitude, and satisfy that the probability of constraint violation does not exceed the constraint violation probability threshold.

[0101] A tubular nonlinear model predictive control is used to perform online rolling solution with warm start within each fixed control cycle. The tubular control is defined by the nominal trajectory and the set of allowable deviations around the nominal trajectory. The set of allowable deviations is determined by the covariance ellipsoid at each prediction time, and the deviations are kept within the covariance ellipsoid by a local feedback law.

[0102] Output the relative velocity reference trajectory and attitude reference trajectory.

[0103] Terminology definition:

[0104] The constraint violation probability threshold is a preset threshold used to limit the maximum probability that an opportunity constraint is allowed to be violated;

[0105] The probability scenario is a set of discrete perturbation sequences that represent possible evolution paths of relative pose perturbation, obtained by sampling in the prediction time domain based on the corrected anisotropic probability tube.

[0106] The scenario-based opportunity constraint is to transform the constraint that was originally expressed in probabilistic form into a set of constraints that are simultaneously satisfied for several probabilistic scenarios, thereby realizing a computable approximate opportunity constraint.

[0107] The tubular nonlinear model predictive control is a control method that, within the framework of nonlinear model predictive control, simultaneously optimizes the nominal control sequence and combines it with the set of allowable deviations around the nominal trajectory to form a "tubular" robust constraint structure, so as to maintain the system state satisfying the constraints when disturbances exist.

[0108] The online rolling solution is a solution method that reconstructs and solves the prediction optimization problem based on the latest state variables in each fixed control cycle, executes the control variables of the current cycle only, and updates the prediction time domain forward.

[0109] The relative velocity reference trajectory is a reference sequence generated in the prediction time domain to indicate the change of the relative velocity of the mobile robot relative to the target workstation over time.

[0110] The attitude reference trajectory is a reference sequence generated in the prediction time domain to indicate the change of the relative attitude of the mobile robot with respect to the target workstation over time.

[0111] The discrete-time nonlinear relative kinematics model is a nonlinear state update model that describes the evolution of the relative pose state between the mobile robot and the target workstation with control inputs such as relative velocity in discrete time steps.

[0112] The perturbation set is the set of possible values ​​of relative pose perturbation determined by the corrected anisotropic probability tube at each prediction time in the prediction time domain.

[0113] The stratified sampling is a sampling method that selects sampling points along the principal axis of the covariance ellipsoid according to a preset level or amplitude to generate perturbation samples;

[0114] The perturbation vector is an offset sampled from the covariance ellipsoid corresponding to a certain prediction time and used to be superimposed on the time-varying mean at that prediction time.

[0115] The rolling optimization problem is a constrained optimization problem that uses the predicted control sequence in the time domain as the decision variable and is repeatedly solved in each fixed control cycle.

[0116] The state constraint is a feasibility restriction imposed on the relative pose state and its derivatives in the prediction time domain.

[0117] The input constraints are feasibility restrictions imposed on the relative velocity and attitude control inputs and their changes in the prediction time domain;

[0118] The minimum approach distance is a constraint that the mobile robot and the target workstation should maintain a distance of not less than a preset safety distance within the predicted time domain;

[0119] The attitude deviation is the amount of deviation of the relative attitude from the preset docking attitude or nominal attitude.

[0120] The upper limit of relative velocity is a preset maximum value that the relative velocity must not exceed within the prediction time domain;

[0121] The rate of change bound is an upper limit constraint on the allowable change in relative velocity or relative attitude between adjacent discrete time steps.

[0122] The warm start is an initialization method in which the optimized solution or its variation from the previous control cycle is used as the initial value for the current cycle during the rolling solution of adjacent control cycles.

[0123] The nominal trajectory is the center trajectory of the predicted state and control obtained by optimization solution under conditions that do not consider or are represented by mean perturbation;

[0124] The allowable deviation set is a set of state deviation ranges that are allowed to exist around the nominal trajectory at each prediction time, and this set is determined by the uncertainty range at the corresponding time.

[0125] The local feedback law is a feedback control relationship used to generate feedback correction control quantities based on the deviation between the actual state and the nominal trajectory, so as to keep the deviation within the allowable deviation set.

[0126] Optionally, S6 includes:

[0127] At the edge nodes, the relative velocity reference trajectory and attitude reference trajectory are taken and interpolated at the boundary moments of the current control cycle to obtain the current reference relative pose;

[0128] The time-aligned visual observations and the time-aligned visual observations from the previous control cycle are input into the feature extraction network to calculate the current image features and the reference image features.

[0129] Based on the current reference relative pose of the camera in-camera participants, the mapping relationship of image features with respect to pose is linearized under the small pose assumption. A least squares objective function consisting of feature alignment error, reference relative pose deviation and pose increment regularization is constructed and solved using Gauss-Newton iteration with robust kernel to obtain the unconstrained pose increment.

[0130] Based on the corrected anisotropic probability tube, the pose increment is scaled and thresholded in each principal axis direction to ensure stable convergence, forming the end pose increment command of the current control cycle.

[0131] Terminology definition:

[0132] The servo target is the target state used to drive the visual servo solution, wherein the servo target includes at least the current reference relative pose determined by the relative velocity reference trajectory and the attitude reference trajectory;

[0133] The boundary time values ​​and interpolation are processes that sample the relative velocity reference trajectory and attitude reference trajectory at boundary times of a fixed control period and perform time interpolation when necessary to obtain the current reference relative pose.

[0134] The current reference relative pose is the reference pose used for visual servo alignment within the current control cycle, which is determined by the relative velocity reference trajectory and the attitude reference trajectory at the time corresponding to the current control cycle.

[0135] The feature extraction network is a neural network or feature operator used to extract image features from temporally aligned visual observations that can be used for alignment and error calculation;

[0136] The image features are feature representations extracted from the image by the feature extraction network, used to characterize the docking target or environmental structure and can be used for matching and alignment;

[0137] The reference image features are target feature representations corresponding to the current reference relative pose, wherein the reference image features are extracted from time-aligned visual observations of the previous control cycle or from a preset reference image.

[0138] The small pose assumption is that the relative pose change amplitude within the current control cycle is within a preset small range, so that the mapping relationship of image features with respect to pose can be approximated by first-order linearization.

[0139] The linearization is a process of performing a first-order Taylor expansion on the nonlinear mapping relationship of image features with respect to pose changes under the small pose assumption to obtain an approximate linear relationship.

[0140] The least squares objective function is an optimization objective used to solve the pose increment, which includes at least an image feature alignment error term and a pose increment regularization term;

[0141] The feature alignment error is the amount of difference between the current image features and the reference image features in the same feature space;

[0142] The pose increment regularization is a penalty term used to limit the magnitude or smoothness of the pose increment, in order to improve the stability of the iterative solution;

[0143] The robust kernel is a robust loss function used in the least squares objective function to reduce the impact of abnormal feature matching errors;

[0144] The Gauss-Newton iteration is an iterative optimization method for solving the least squares objective function, which reduces the feature alignment error by iteratively updating the pose increment.

[0145] The unconstrained pose increment is the pose change obtained directly from the least squares objective function and has not yet been subject to step size scaling or threshold limitation based on uncertainty constraints.

[0146] The end-effector pose increment command is a pose increment control command formed by scaling the unconstrained pose increment with a step size and limiting it with a threshold. It is used to drive the end-effector of the mobile robot to perform docking fine-tuning.

[0147] Optionally, the S7 includes:

[0148] At the edge node, the end-effector pose increment command is converted into an end-effector velocity command according to a fixed control cycle, and the end-effector velocity command is input into the compliant controller;

[0149] The compliant controller calculates the desired interaction force based on an impedance model and generates an unrestricted control quantity. The impedance model includes a stiffness matrix and a damping matrix, which are directionally scaled according to the scale of the corrected anisotropic probability tube in each principal axis direction.

[0150] Gain limiting and force limiting are performed on the unrestricted control quantity, wherein gain limiting is used to keep the stiffness matrix and damping matrix within a preset positive definite range, and force limiting is used to limit the desired interaction force within a safe threshold.

[0151] The restricted control quantity and the end-effector speed command are combined into an actuator control command, which is then sent from the edge node to the mobile robot at a fixed control cycle to perform the docking action.

[0152] Output actuator control commands.

[0153] Terminology definition:

[0154] The compliance controller is a control module used to generate compliance control quantities based on end motion commands and interactive mechanical models during docking or insertion processes, in order to reduce impact and improve contact stability.

[0155] The end-effector velocity command is an end-effector linear velocity and angular velocity control command obtained by converting the end-effector pose increment command according to a fixed control cycle.

[0156] The impedance model is a dynamic or mechanical model that describes the relationship between end-effector pose deviation and end-effector interaction force, and is used to generate the desired interaction force or control quantity.

[0157] The stiffness matrix is ​​a matrix parameter in the impedance model used to characterize the magnitude and directional coupling relationship of the restoring force / moment caused by end-effector pose deviation;

[0158] The damping matrix is ​​a matrix parameter in the impedance model used to characterize the magnitude and directional coupling relationship of the damping force / moment caused by the end velocity;

[0159] The directional scaling is a process of differentially adjusting the gains of the stiffness matrix and damping matrix in the corresponding directions based on the uncertainty scale of the corrected anisotropic probability tube in different principal axis directions.

[0160] The unrestricted control quantity is the control output calculated based on the impedance model and has not yet undergone gain limiting and force limiting processing.

[0161] The gain limitation is a process of restricting the range of parameter values ​​of the stiffness matrix and damping matrix, so as to keep the stiffness matrix and damping matrix within a preset positive definite range and avoid control instability;

[0162] The force limiting is a process of applying an upper limit constraint to the desired interaction force or the output force / torque corresponding to the control quantity, so that the interaction force does not exceed a preset safety threshold.

[0163] The actuator control command is a control instruction used to drive the mobile robot actuator to output motion and / or force control, which is synthesized from a limited control quantity and an end-effector speed command;

[0164] The "download" refers to the process by which the edge node sends the actuator control command to the mobile robot control system according to a fixed control cycle and triggers its execution.

[0165] Optionally, the conditional diffusion short-time prediction model performs forward diffusion and reverse denoising on SE(3) by mapping the relative pose to the tangent space of the Lie algebra se(3), and uses equal-variable noise injection to maintain the invariance of translation and rotation. The reverse denoising uses a distilled K-step solver, where K does not exceed a preset real-time upper limit.

[0166] Optionally, online reliability calibration is performed on the anisotropic probability tube constructed from the time-varying mean and time-varying covariance: statistically predict the consistency between the coverage and measurement residual within a sliding window, calculate the scaling factor β≥1 for each principal axis direction, and conservatively expand the covariance ellipsoid according to the scaling factor.

[0167] The beneficial effects of this invention are:

[0168] 1. Improve dynamic docking accuracy and success rate: By aligning the data of cameras, lidar, inertial measurement units, ultra-wideband ranging, station encoders and programmable logic controllers under a unified time reference, and using a three-dimensional special Euclidean group and other variable multi-source fusion networks to output relative pose estimation and covariance, the systematic errors introduced by asynchronous sampling and link delay are reduced. This enables stable and reliable relative pose estimation to be obtained even under the conditions of slight movement or drift of the target station, thereby improving docking accuracy and docking success rate.

[0169] 2. Achieving controllable safety constraints for drift uncertainty: The conditional diffusion short-time prediction model directly outputs the time-varying mean and time-varying covariance in the prediction time domain and constructs anisotropic probability tubes; combined with joint correction of prediction and measurement based on score functions, the uncertainty changes with time and direction can be effectively characterized and corrected online. On this basis, scenario-based chance constraints are constructed and a tubular nonlinear model is used for predictive control and rolling solution, making the constraint violation probability controllable, reducing the risk of collisions and misinsertion, while avoiding excessive conservatism caused by using a fixed safety margin;

[0170] 3. Improve the stability and safety of the contact phase: By combining the reference trajectory obtained by model predictive control with depth vision servo, feature errors are minimized in real time and the end pose increment command is output. At the same time, the pose increment is directionally scaled and thresholded according to the corrected anisotropic probability tube. Then, the gain is limited and the force is limited by the compliant controller. This can suppress oscillation and impact during the contact or insertion phase, and achieve smoother and safer compliant docking. Attached Figure Description

[0171] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings:

[0172] Figure 1 This is a flowchart of a precise docking method for mobile robots based on edge computing proposed in this invention. Detailed Implementation

[0173] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0174] refer to Figure 1 A precise docking method for mobile robots based on edge computing includes:

[0175] S1. Receive raw data from the camera, lidar, inertial measurement unit, ultra-wideband ranging module of the mobile robot, and encoder and programmable logic controller of the target station. Perform time alignment based on a unified time reference to obtain time-aligned visual observation, lidar observation, inertial observation, ultra-wideband observation, encoder reading and programmable logic controller status.

[0176] S2. Using visual observation, laser observation, inertial observation, and ultra-wideband observation as features, and encoder readings and programmable logic controller states as constraints, input a three-dimensional special Euclidean group and other variable multi-source fusion network to obtain the relative pose estimation and pose covariance of the current control cycle.

[0177] S3. In the most recent time window, the relative pose estimation and pose covariance are spliced ​​together, and the encoder reading and programmable logic controller state are normalized and used as conditions to input the conditional diffusion short-time prediction model to obtain the time-varying mean and time-varying covariance in the prediction time domain. The covariance ellipsoids of each prediction time are connected along the time axis to construct an anisotropic probability tube.

[0178] S4. Using the anisotropic probability tube as a priori and the relative pose estimation and pose covariance of the current control cycle as measurements, the time-varying mean and time-varying covariance are jointly corrected based on the scoring function, and the corrected anisotropic probability tube is reconstructed accordingly.

[0179] S5. Based on the corrected anisotropic probability tube as the constraint, multiple probability scenarios are extracted according to the preset constraint violation probability threshold, scenario-based opportunity constraints are established, and online rolling solution is obtained by using the tube nonlinear model predictive control to obtain the relative velocity reference trajectory and attitude reference trajectory.

[0180] S6. Using the relative velocity reference trajectory and attitude reference trajectory as servo targets, perform feature alignment of time-aligned visual observations within the current control cycle, and solve the end pose increment command by minimizing the feature error.

[0181] S7. Input the end-effector pose increment command into the compliant controller. After gain limiting and force limiting processing, generate actuator control commands and send them to the mobile robot to perform docking according to the fixed control cycle.

[0182] In this specific embodiment, S1 includes:

[0183] Edge nodes prioritize clock synchronization with the plant time source via a precision time protocol to establish a unified time reference. When the precision time protocol is unavailable or the synchronization quality does not meet a preset threshold, the hardware clock of the edge node is used as the unified time reference.

[0184] Edge nodes receive raw data from the mobile robot's camera, LiDAR, inertial measurement unit, and ultra-wideband ranging module, as well as from the encoder and programmable logic controller at the target workstation. They record a local timestamp for each channel. Then, for the clock offset and frequency deviation between each channel's local timestamp and the unified time reference, a sliding window linear regression is used for estimation, combined with round-trip delay measurements of the wireless link for offset compensation. This yields a time mapping function that maps the local time of each channel to the unified time reference time axis. The time mapping function can be expressed as:

[0185] ;

[0186] in, This represents the mapped unified time base timestamp, with the unit being seconds. Indicates the first Time mapping function for each channel This indicates the channel index and is used to distinguish between cameras, LiDAR, inertial measurement units, ultra-wideband ranging modules, encoders, and programmable logic controllers. Indicates the first Each channel records a local timestamp in seconds. Indicates the first Each channel has a frequency scaling factor relative to a unified time base and is used to characterize frequency deviation. Indicates the first Each channel has a time offset relative to a unified time base, and the unit is seconds. Indicates the edge node and the first The round-trip delay between each channel data source is measured in seconds and passed through... Achieve approximate compensation for one-way time delay;

[0187] After completing the time mapping, the edge nodes resample and align each channel at boundary times with a fixed control period, denoted as . The control cycle number is denoted as , No. The boundary time of each control cycle is denoted as And satisfy and The periodic correspondence, in each At this point, the camera channel should be selected based on the closest timestamp. The image frames are processed, and the line-level sampling time difference introduced by the rolling shutter is time-corrected to output time-aligned visual observations. The lidar channel performs motion distortion correction on the point cloud within a scanning cycle according to point-level timestamps and... The point cloud of this period is output to form time-aligned laser observations. The inertial measurement unit channel integrates or averages the acceleration and angular velocity within the current control cycle to obtain time-aligned inertial observations. The ultra-wideband ranging module channel... Nearby ranging values ​​can be selected or time interpolation can be used to obtain time-aligned ultrawideband observations. The encoder and programmable logic controller channels are in... The nearest neighbor hold strategy is used to obtain time-aligned encoder readings and time-aligned programmable logic controller states, respectively, thereby outputting time-aligned visual observations, time-aligned laser observations, time-aligned inertial observations, time-aligned ultrawideband observations, time-aligned encoder readings, and time-aligned programmable logic controller states for subsequent multi-source fusion.

[0188] In this specific embodiment, S2 includes:

[0189] After obtaining time-aligned visual observations, time-aligned laser observations, time-aligned inertial observations, and time-aligned ultra-wideband observations, the edge nodes first input the time-aligned visual observations and time-aligned laser observations into the static geometry branch to extract the geometric structure and cross-modal correspondence features of the docking target. Then, the time-aligned inertial observations and time-aligned ultra-wideband observations are input into the dynamic cue branch to extract motion state and distance constraint features. The features of the two branches are then weighted and fused according to the observation quality weights. The observation quality weights are calculated by image sharpness and motion blur indices, laser echo intensity and point cloud consistency indices, and ultra-wideband non-line-of-sight scores to suppress the influence of low-quality observations on the fusion results.

[0190] Meanwhile, the edge nodes use time-aligned encoder readings as relative kinematic constraint inputs and time-aligned programmable logic controller states as workstation state constraint inputs, so that the fusion inference process is consistent with the workstation action stage, enable state, and feasible relative displacement range. In the fusion stage, micro-correction is performed on the extrinsic parameter bias across sensors to correct the pose extrinsic parameter deviation between different sensor coordinate systems within a small range, thereby improving the feature correspondence.

[0191] Under the combined influence of the above inputs and constraints, the three-dimensional special Euclidean group and other variable multi-source fusion network outputs the relative pose estimate and pose covariance for the current control cycle, which can be expressed as:

[0192] ;

[0193] in, Indicates the sequence number of the current control cycle. Indicates the first The relative pose estimate output in each control cycle is used to characterize the pose relationship of the mobile robot relative to the target workstation. Indicates and The corresponding pose covariance is used to characterize the uncertainty of the relative pose estimation. This represents the three-dimensional special Euclidean group equal-variable multi-source fusion network. This represents the network parameters of the three-dimensional special Euclidean group equal-variable multi-source fusion network. Indicates the first Time-aligned visual observations for each control cycle Indicates the first Time-aligned laser observations over a control cycle Indicates the first Time-aligned inertial observations for each control cycle Indicates the first Time-aligned ultrawideband observations over one control cycle Indicates the first Time-aligned encoder readings for each control cycle Indicates the first The time-aligned state of the programmable logic controller for each control cycle.

[0194] In this specific embodiment, S3 includes:

[0195] Executed by the edge node within the current control cycle, the edge node first concatenates the relative pose estimation and pose covariance in chronological order within the nearest time window of the fixed control cycle. This nearest time window can contain a preset number of control cycles or a preset duration, thus forming a temporal input characterizing recent relative pose evolution and uncertainty changes. Simultaneously, the edge node performs dimensional uniformity and range normalization on the encoder readings and programmable logic controller (PLC) states aligned with the time window, ensuring that different physical quantities participate in prediction and inference under a unified numerical scale. The concatenated result, together with the normalized encoder readings and PLC states, forms a conditional input, denoted as […]. ,in, Indicates the first The conditional input data corresponding to each control cycle. Indicates the sequence number of the current control cycle;

[0196] Subsequently, the edge nodes will The input conditional diffusion short-time prediction model generates the time-varying mean and time-varying covariance of the relative pose distribution in the prediction time domain through forward diffusion and reverse denoising sampling. The number of reverse denoising steps and noise intensity are set according to the fixed control cycle to meet the real-time requirements and ensure that the output is completed within a single control cycle.

[0197] In this embodiment, the output of the conditional diffusion short-time prediction model is expressed as:

[0198] ;

[0199] in, This represents the conditional diffusion short-time prediction model. This represents the model parameters of the conditional diffusion short-time prediction model. This indicates the number of prediction steps corresponding to the prediction time domain. Represents the prediction step index and satisfies Indicates the first The time-varying mean of the relative pose probability distribution at each prediction time. Indicates the first Time-varying covariance of the relative pose probability distribution at each prediction time;

[0200] Edge nodes are based on each prediction time. and Build with Centered on, with The determined covariance ellipsoid is used as the spatiotemporal channel of the cross section, and the covariance ellipsoids of each prediction time are connected sequentially along the time axis to form an anisotropic probability tube, thereby providing a relative pose uncertainty expression that can simultaneously characterize the temporal variability and directional difference for subsequent steps.

[0201] In this specific embodiment, S4 includes:

[0202] Executed by the edge node within each fixed control cycle, the edge node uses the anisotropic probability tube as a priori and estimates the relative pose for the current control cycle. and its pose covariance As a measurement quantity, among which This indicates the current control cycle number, and the anisotropic probability tube is determined by the time-varying mean in the prediction time domain. With time-varying covariance Characterization, Represents the prediction step index and contains Corresponding to the boundary time of the current control cycle;

[0203] During joint correction, edge nodes call the conditional diffusion short-time prediction model in the prior prediction time domain to calculate the gradient of the prior log probability as a score function, and this score function is denoted as... ,in Used to indicate in the The direction of correction to improve the prior probability density at each prediction time;

[0204] Meanwhile, edge nodes Calculate the measurement residuals ,in To be and The residual vector obtained under the same pose error expression, and based on and Construct an equivalent gain matrix to perform a priori update, the first joint correction being:

[0205] ;

[0206] in, Indicates the updated number The time-varying mean of each prediction time, Indicates the updated number The time-varying covariance at each prediction time, Indicates the first The equivalent gain matrix at each prediction time. This represents the measurement residual vector formed by the measurement and the prior mean at the current control cycle boundary time. This represents the non-negative coefficient used to balance the weights of the measurement residual term and the scoring function term. Indicates the first The score function vector at each prediction time. Indicates and identity matrices with consistent dimensions This represents the matrix inversion operation. This represents the pose covariance corresponding to the measurement. This represents the time-varying covariance corresponding to the prior knowledge.

[0207] After completing the first joint correction, the edge nodes retain their predicted time index in the time domain, only using... and The covariance ellipsoid is reconstructed at each prediction time and connected along the time axis to obtain the corrected anisotropic probability tube.

[0208] In this specific embodiment, S5 includes:

[0209] The process is executed online in a rolling manner by edge nodes within each fixed control cycle. The edge nodes use a discrete-time nonlinear relative kinematic model as the controlled object and employ a corrected anisotropic probability tube to determine the set of disturbances at each prediction time within the prediction time domain. The corrected anisotropic probability tube is derived from the updated time-varying mean. With the updated time-varying covariance Characterization, in which Indicates the current control cycle number. This indicates the number of prediction steps in the prediction time domain. Represents the prediction step index and satisfies ;

[0210] To transform chance-constraint computation into a deterministic approximation that can be solved in real time, edge nodes are based on a preset constraint violation probability threshold. Layered sampling is performed along the principal axis of the covariance ellipsoid corresponding to each prediction step to generate... Let there be 1 probability scenario and denote the scenario index as . And satisfy Each probability scenario in the 1st The perturbation vector at each prediction time is denoted as... And superimpose the disturbance vector onto The above describes the construction of relative pose perturbation samples for this scene, specifically:

[0211] ;

[0212] in, Indicates the first The probability scenario in the th probability scenario is... The relative pose perturbation samples corresponding to each predicted time. Indicates the first The updated time-varying mean at each predicted time point. Indicates by the first The covariance ellipsoid sampling at the prediction time obtained the first A scene perturbation vector Indicates the first The updated time-varying covariance at each prediction time. Indicates the probability threshold of constraint violation. The determined ellipsoidal scale parameters are used to limit the range of sampling perturbation amplitude. This indicates the transpose operation. This represents the matrix inversion operation;

[0213] After obtaining the probabilistic scenarios, the edge nodes construct a rolling optimization problem using the relative velocity sequence and attitude sequence in the prediction time domain as decision variables. The cost function includes a target tracking term for relative pose, a velocity matching term, a control increment penalty term, and a terminal stabilization term. Its constraints include state constraints and input constraints that hold simultaneously for all probabilistic scenarios. The state constraints include at least minimum approach distance, attitude deviation, and a relative velocity upper limit. The input constraints include at least a bound on the rate of change of relative velocity and a bound on the rate of change of attitude. This is further demonstrated by... Simultaneously apply constraints to multiple probability scenarios to ensure that the probability of constraint violation does not exceed the constraint violation probability threshold. ;

[0214] The edge nodes employ tubular nonlinear model predictive control to perform a warm-start online solution to the rolling optimization problem. The tubular control is defined by the nominal trajectory and the set of allowable deviations around the nominal trajectory. The set of allowable deviations is determined by the corresponding covariance ellipsoid at each prediction time. The actual deviations are kept within the covariance ellipsoid by a local feedback law to enhance robustness to disturbances and constraint retention.

[0215] Output the relative velocity reference trajectory and attitude reference trajectory.

[0216] In this specific embodiment, S6 includes:

[0217] Executed by the edge nodes within each fixed control cycle, the edge nodes take values ​​and perform time interpolation on the relative velocity reference trajectory and attitude reference trajectory at the boundary moments of the current control cycle, thereby obtaining the current reference relative pose for visual servoing in this cycle. ,in Indicates the current control cycle number. Indicates the first The reference relative pose corresponding to each control cycle is used to characterize the reference pose relationship between the mobile robot and the target workstation.

[0218] Subsequently, the edge node will... Time-aligned visual observations of each control cycle Visual observations aligned with the time of the previous control cycle Input a feature extraction network to compute current image features and reference image features, wherein the current image features are denoted as... The reference image features are denoted as ,in Indicates the first Time-aligned visual observations for each control cycle Indicates the first Time-aligned visual observations for each control cycle Indicates by The extracted image features, Indicates by Or obtained by extracting from a preset reference image and with Corresponding reference image features;

[0219] Under the small pose assumption, edge nodes are based on the camera intrinsic parameter matrix. and The mapping relationship between image features and pose is linearized to obtain a first-order approximation of the feature alignment error relative to the pose increment. A least-squares objective function is then constructed, and the unconstrained pose increment is obtained through Gaussian-Newton iteration with a robust kernel. ,in This represents the camera intrinsic parameter matrix and is used to correlate 3D geometry with pixel measurements. Indicates the first The end pose increment variable to be solved within each control cycle;

[0220] The least squares objective function can be expressed as:

[0221] ;

[0222] in This represents a robust kernel function used to reduce the impact of outlier errors on the solution results. Represents the L2 norm, Indicates the first Each control cycle is composed of and The Jacobian matrix of the defined feature error with respect to the pose increment. This represents the non-negative weight coefficient of the pose increment regularization term;

[0223] In obtaining Subsequently, the edge nodes further update the time-varying covariance corresponding to the corrected anisotropic probability tube in the current control cycle. Along the principal axis of its covariance ellipsoid By performing step scaling and thresholding, the unconstrained pose increment is transformed into an end-effector pose increment command that meets the requirements of stable convergence. ,in Indicates the first The updated time-varying covariance corresponding to each control cycle Indicates the first The end pose increment command output in each control cycle is used as the input for subsequent compliant control.

[0224] In this specific embodiment, S7 includes:

[0225] Executed by the edge node within each fixed control cycle, the edge node receives the end-effector pose increment command. and based on a fixed control cycle Will Convert to end velocity command ,in Indicates the current control cycle number. Indicates the first End-of-cycle pose increment command Indicates a fixed control cycle. Indicates the first The end-of-cycle speed command includes both end-of-cycle linear velocity and angular velocity;

[0226] Then The input is a compliant controller, which calculates the desired interaction force based on an impedance model and generates an unrestricted control quantity, wherein the unrestricted control quantity is:

[0227] ;

[0228] in Indicates the first Each control cycle is determined by the unrestricted control quantity output from the impedance model. Indicates the first Stiffness matrix for each control cycle Indicates the first Damping matrix for each control cycle, Indicates the first The end pose error vector of each control cycle Indicates the first The vector of the rate of change of pose error at the end of each control cycle is used to characterize the trend of error change over time.

[0229] in, and Based on the updated time-varying covariance of the corrected anisotropic probability transistor in the current control cycle The determined covariance ellipsoid principal axis scale is directionally scaled so that the principal axis direction with greater uncertainty corresponds to smaller stiffness and damping gain, while the principal axis direction with less uncertainty corresponds to larger stiffness and damping gain, thus taking into account both compliance and convergence during the docking contact stage.

[0230] In obtaining Then, gain limiting and force limiting are performed on it, where gain limiting is used to... and The force limiting process is used to limit the control output corresponding to the expected interaction force within a preset positive definite range to ensure control stability, thereby suppressing the risk of impact and collision.

[0231] The edge node will process the control quantity and end velocity command after gain limiting and force limiting. Synthesize the data to generate actuator control commands, and execute them according to a fixed control cycle. The actuator control command is sent to the mobile robot to perform the docking action, and the actuator control command is output.

[0232] In this specific embodiment,

[0233] The conditional diffusion short-time prediction model in When performing generative short-time prediction of relative poses, the prediction variables are first mapped from the group space to Lie algebras. The tangent space facilitates forward diffusion and reverse denoising sampling, and the mapping can be performed on relative poses. Implement logarithmic mapping:

[0234] ,in This represents the tangent space pose vector corresponding to the relative pose and is used for diffusion calculations in the tangent space. This represents the relative pose of the mobile robot with respect to the target workstation and belongs to a special three-dimensional Euclidean group. Indicates from arrive The logarithmic mapping operator, express The corresponding Lie algebra tangent space;

[0235] After completing the above mapping, the conditional diffusion short-time prediction model performs the following within the tangent space: The model performs forward diffusion to gradually inject noise and adopts an equivariant noise injection strategy in the reverse denoising stage to keep the statistical characteristics of the noise consistent under rigid body transformations of translation and rotation. This allows the model to maintain the equivariance and consistency of the output prediction distribution even when the input observations undergo consistent rigid body transformations.

[0236] To meet the real-time requirements of edge nodes, reverse denoising does not employ high-step iterations but instead uses distilled... The step solver generates samples, where The discrete step number for inverse denoising and satisfying , The real-time performance limit is preset and determined by the allowable calculation delay within a fixed control period;

[0237] Finally, the model organizes the predicted distribution parameters obtained in the tangent space into the time-varying mean and time-varying covariance in the prediction time domain and outputs them. The time-varying mean and time-varying covariance are consistent with the expression of the anisotropic probability tube constructed in step S3. At the same time, when it is necessary to return to the group space for use, the tangent space result is mapped to SE (3) through exponential mapping to ensure consistency with the pose expression of subsequent control and constraint calculation.

[0238] In this specific embodiment,

[0239] Online reliability calibration is performed on the anisotropic probability transistor, with an edge node maintenance length of [missing information]. The sliding window, in which This represents the number of most recent control periods included in the statistics and is a positive integer. Edge nodes will measure residuals within each control period. With the corresponding updated time-varying covariance Write to the sliding window, where Indicates the current control cycle number. This represents the measurement residual vector formed by the relative pose estimate of the current control cycle and the prior time-varying mean. This represents the updated time-varying covariance matrix corresponding to the current control cycle;

[0240] Within the sliding window, the edge nodes use the consistency between the coverage of the probability tube and the measurement residual in each principal axis direction as a statistic, and calculate the scaling factor for each principal axis direction separately. ,in Indicates the main axis direction index and satisfies The dimension of the relative pose error vector is represented and, in one embodiment, is taken as... , satisfy Furthermore, the impact increases when the coverage rate is lower than the target coverage rate or the residual consistency deviates from the expected consistency. This allows for a conservative correction of the underestimation of uncertainty.

[0241] In obtaining Subsequently, the edge nodes perform conservative dilation on the covariance ellipsoid corresponding to each prediction step in the prediction time domain, specifically by adjusting the updated time-varying covariance. Scale along the principal axis and obtain the calibrated time-varying covariance. ,in Represents the prediction step index and satisfies The calibration process, representing the number of prediction steps in the prediction time domain, can be expressed as:

[0242] ;

[0243] in Indicates to The principal axis direction matrix obtained by eigenvalue decomposition has column vectors that are the principal axis directions of the covariance ellipsoid. The operator represents a diagonal matrix formed by the elements within the parentheses. express In the The eigenvalues ​​along each principal axis are used to characterize the uncertainty scale in that direction. This represents the result of the sliding window statistics. Scaling factor in the principal axis direction, Indicates the transpose operation;

[0244] Edge nodes then Replace the original To reconstruct the covariance ellipsoid and update the anisotropic probability tube, so that the probability tube has a more conservative and reliable constraint basis in subsequent scenario-based chance constraints and tube-shaped nonlinear model predictive control.

[0245] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

[0246] This invention addresses the dynamic docking challenges caused by micro-motions or drifts at the target workstation. It constructs a closed-loop technology chain consisting of "data alignment with a unified time reference, multi-source fusion estimation, short-time prediction and correction, optimized control under uncertainty constraints, and visual servoing and compliant docking execution." By performing time-series consistency processing on multi-source observations and workstation-side signals at edge nodes, systematic errors introduced by clock bias and link delays are reduced. Then, a three-dimensional special Euclidean group is used to obtain the relative pose and its covariance, enabling the docking control to obtain reliable state quantities that can be used for constraint inference. Furthermore, based on conditional diffusion short-time prediction, the time-varying mean and covariance in the prediction time domain are obtained, and the reference trajectory is solved in a rolling manner within the predictive control framework of a tubular nonlinear model under scenario-based opportunistic constraints. This allows the control process to balance docking accuracy, dynamic response, and safety margin while keeping the probability of constraint violation under control. Finally, by combining feature error minimization of deep visual servoing with force limiting processing of compliant control, stability and safety during the contact and insertion stages are ensured, thereby improving the docking success rate under dynamic disturbance conditions and reducing collision risk.

[0247] This invention improves the algorithm structure to address key issues in dynamic docking: First, by directly outputting the time-varying mean and covariance of the prediction time domain from the conditional diffusion model, uncertainties are connected along the time axis by a covariance ellipsoid to form an anisotropic probability tube. This can express the differences in drift in different directions and at different time scales, avoiding overly conservative or insufficiently constrained assumptions caused by fixed margins or isotropic assumptions. Second, a joint correction of prediction and measurement based on a scoring function is introduced. While keeping the prediction time index unchanged, the time-varying mean and covariance are updated once, integrating generative prediction with real-time measurement consistency, improving the usability and reliability of the probability tube. Third, the probability tube is transformed into a scenario-based chance constraint and combined with a tube-shaped control structure. In real-time rolling solution, disturbance uncertainty and input state constraints are handled simultaneously, improving the robustness and stability of the dynamic docking process. These improvements collectively promote the invention to more effectively achieve high-precision and high-safety dynamic docking technology.

Claims

1. A precise docking method for mobile robots based on edge computing, characterized in that, include: S1. Receive raw data from the camera, lidar, inertial measurement unit, ultra-wideband ranging module of the mobile robot, and encoder and programmable logic controller of the target station. Perform time alignment based on a unified time reference to obtain time-aligned visual observation, lidar observation, inertial observation, ultra-wideband observation, encoder reading and programmable logic controller status. S2. Using visual observation, laser observation, inertial observation, and ultra-wideband observation as features, and encoder readings and programmable logic controller states as constraints, input a three-dimensional special Euclidean group and other variable multi-source fusion network to obtain the relative pose estimation and pose covariance of the current control cycle. S3. In the most recent time window, the relative pose estimation and pose covariance are spliced ​​together, and the encoder reading and programmable logic controller state are normalized and used as conditions to input the conditional diffusion short-time prediction model to obtain the time-varying mean and time-varying covariance in the prediction time domain. The covariance ellipsoids of each prediction time are connected along the time axis to construct an anisotropic probability tube. S4. Using the anisotropic probability tube as a priori and the relative pose estimation and pose covariance of the current control cycle as measurements, the time-varying mean and time-varying covariance are jointly corrected based on the scoring function, and the corrected anisotropic probability tube is reconstructed accordingly. S5. Based on the corrected anisotropic probability tube as the constraint, multiple probability scenarios are extracted according to the preset constraint violation probability threshold, scenario-based opportunity constraints are established, and online rolling solution is obtained by using the tube nonlinear model predictive control to obtain the relative velocity reference trajectory and attitude reference trajectory. S6. Using the relative velocity reference trajectory and attitude reference trajectory as servo targets, perform feature alignment of time-aligned visual observations within the current control cycle, and solve the end pose increment command by minimizing the feature error. S7. Input the end-effector pose increment command into the compliant controller. After gain limiting and force limiting processing, generate actuator control commands and send them to the mobile robot to perform docking according to the fixed control cycle.

2. The precise docking method for mobile robots based on edge computing according to claim 1, characterized in that, S1 includes: The edge nodes synchronize their clocks with the plant's time source via a precision time protocol to establish a unified time base. When the precision time protocol is unavailable, the edge nodes' hardware clocks are used as the unified time base. The edge node receives raw data from the mobile robot's camera, LiDAR, inertial measurement unit, ultra-wideband ranging module, as well as from the encoder and programmable logic controller at the target workstation, and records the local timestamp of each channel. For the local timestamp of each channel, the time offset and frequency deviation between the local time and the unified time reference are estimated based on the sliding window linear regression, and the time offset is compensated by combining the round-trip delay measurement of the wireless link, so as to obtain the time mapping function from the local time to the unified time reference. The time mapping function is used to map the timestamps of each channel to a unified time reference. At the boundary moments of a fixed control cycle, each channel is resampled and aligned. The camera obtains time-aligned visual observation by selecting the image frame closest to the boundary moment and performing rolling shutter time correction. The lidar performs motion distortion removal processing on the point cloud within a scanning cycle based on the timestamp and outputs the point cloud of that cycle at the boundary moment to obtain time-aligned lidar observation. The inertial measurement unit integrates or averages the acceleration and angular velocity within the current control cycle to obtain time-aligned inertial observation. The ultra-wideband ranging module selects the ranging value or performs time interpolation near the boundary moment to obtain time-aligned ultra-wideband observation. The encoder and programmable logic controller use the nearest neighbor hold strategy at the boundary moment to obtain time-aligned encoder readings and time-aligned programmable logic controller states, respectively. Output time-aligned visual observations, time-aligned laser observations, time-aligned inertial observations, time-aligned ultrawideband observations, time-aligned encoder readings, and time-aligned programmable logic controller states.

3. The precise docking method for mobile robots based on edge computing according to claim 1, characterized in that, S2 include: At the edge nodes, time-aligned visual observations, time-aligned laser observations, time-aligned inertial observations, and time-aligned ultrawideband observations are used as feature extraction inputs, and time-aligned encoder readings and time-aligned programmable logic controller states are used as constraint inputs, which are then input into a three-dimensional special Euclidean group and other variable multi-source fusion network. The three-dimensional special Euclidean group variable multi-source fusion network includes a static geometric branch and a dynamic cue branch. The static geometric branch extracts geometric features from time-aligned visual observations and time-aligned laser observations, while the dynamic cue branch extracts motion and distance features from time-aligned inertial observations and time-aligned ultra-wideband observations. The two branches are then fused using feature weighting based on observation quality weights. These observation quality weights are calculated based on image sharpness and motion blur indices, laser echo intensity and point cloud consistency indices, and ultra-wideband non-line-of-sight scores. The three-dimensional special Euclidean group equal variable multi-source fusion network applies time-aligned encoder readings as relative kinematic constraints and time-aligned programmable logic controller states as workstation state constraints during the fusion phase. It also performs micro-correction on the extrinsic parameter bias across sensors to correct the feature correspondence. After completing the above fusion, the relative pose estimate of the current control cycle and the pose covariance corresponding to the relative pose estimate are output.

4. The precise docking method for mobile robots based on edge computing according to claim 1, characterized in that, S3 include: At the edge node, the relative pose estimation and pose covariance are concatenated in chronological order within the nearest time window of a fixed control cycle. The time-aligned encoder readings and time-aligned programmable logic controller states are then dimensionally consistent and range-normalized and combined with the concatenation result to form the conditional input. The conditions are input into the conditional diffusion short-time prediction model. The time-varying mean and time-varying covariance of the relative pose are calculated in the prediction time domain through forward diffusion and reverse denoising sampling. The number of steps and noise intensity of the reverse denoising are set according to a fixed control period to meet the real-time requirements. Based on the time-varying mean and time-varying covariance at each prediction time, a spatiotemporal channel is constructed with the time-varying mean as the center and the covariance ellipsoid determined by the time-varying covariance as the cross section, and the covariance ellipsoid is connected along the time axis to obtain an anisotropic probability tube.

5. The precise docking method for mobile robots based on edge computing according to claim 1, characterized in that, S4 includes: At the edge node, the anisotropic probability tube is used as the prior, and the relative pose estimate and its pose covariance of the current control cycle are used as the measurement. The prior is updated once based on the joint correction of prediction and measurement based on the score function. The joint correction of prediction and measurement based on the scoring function includes: calculating the gradient of the prior log probability in the prediction time domain corresponding to the prior using the conditional diffusion short-time prediction model as the scoring function; constructing an equivalent gain matrix based on the measurement residual and the pose covariance of the measurement and the time-varying covariance of the prior; performing a weighted update on the time-varying mean of the prior and a shrinkage update on the time-varying covariance of the prior, while keeping the time index of the prediction time domain unchanged. Based on the updated time-varying mean and time-varying covariance, the covariance ellipsoid is reconstructed at each prediction time and connected along the time axis to obtain the corrected anisotropic probability tube.

6. The precise docking method for mobile robots based on edge computing according to claim 1, characterized in that, S5 include: At the edge nodes, a discrete-time nonlinear relative kinematics model is used as the controlled object, and the corrected anisotropic probability tube is used to determine the set of disturbances at each time point in the prediction time domain. Based on the preset constraint violation probability threshold, the covariance ellipsoid corresponding to each prediction time of the corrected anisotropic probability tube is sampled in layers along the principal axis to generate several probability scenarios. The probability scenarios at each prediction time are obtained by superimposing the time-varying mean and the perturbation vector of the covariance ellipsoid at that time. Using the relative velocity sequence and attitude sequence in the prediction time domain as decision variables, a rolling optimization problem is constructed. The cost function of the rolling optimization problem includes a target tracking term for relative pose, a velocity matching term, a control increment penalty term, and a terminal stabilization term. The constraints of the rolling optimization problem include state constraints and input constraints that are simultaneously valid for all probability scenarios. The state constraints include at least a minimum approach distance, an attitude deviation, and a relative velocity upper limit. The input constraints include at least a bound on the rate of change of relative velocity and a bound on the rate of change of attitude, and satisfy that the probability of constraint violation does not exceed the constraint violation probability threshold. A tubular nonlinear model predictive control is used to perform online rolling solution with warm start within each fixed control cycle. The tubular control is defined by the nominal trajectory and the set of allowable deviations around the nominal trajectory. The set of allowable deviations is determined by the covariance ellipsoid at each prediction time, and the deviations are kept within the covariance ellipsoid by a local feedback law. Output the relative velocity reference trajectory and attitude reference trajectory.

7. A precise docking method for mobile robots based on edge computing according to claim 1, characterized in that, S6 include: At the edge nodes, the relative velocity reference trajectory and attitude reference trajectory are taken and interpolated at the boundary moments of the current control cycle to obtain the current reference relative pose; The time-aligned visual observations and the time-aligned visual observations from the previous control cycle are input into the feature extraction network to calculate the current image features and the reference image features. Based on the current reference relative pose of the camera in-camera participants, the mapping relationship of image features with respect to pose is linearized under the small pose assumption. A least squares objective function composed of feature alignment error, reference relative pose deviation and pose increment regularization is constructed and solved using Gauss-Newton iteration with robust kernel to obtain the unconstrained pose increment. Based on the corrected anisotropic probability tube, the pose increment is scaled and thresholded in each principal axis direction to ensure stable convergence, forming the end pose increment command of the current control cycle.

8. The precise docking method for mobile robots based on edge computing according to claim 1, characterized in that, S7 includes: At the edge node, the end-effector pose increment command is converted into an end-effector velocity command according to a fixed control cycle, and the end-effector velocity command is input into the compliant controller; The compliant controller calculates the desired interaction force based on an impedance model and generates an unrestricted control quantity. The impedance model includes a stiffness matrix and a damping matrix, which are directionally scaled according to the scale of the corrected anisotropic probability tube in each principal axis direction. Gain limiting and force limiting are performed on the unrestricted control quantity, wherein gain limiting is used to keep the stiffness matrix and damping matrix within a preset positive definite range, and force limiting is used to limit the desired interaction force within a safe threshold. The restricted control quantity and the end-effector speed command are combined into an actuator control command, which is then sent from the edge node to the mobile robot at a fixed control cycle to perform the docking action. Output actuator control commands.

9. A precise docking method for mobile robots based on edge computing according to claim 4, characterized in that, The conditional diffusion short-time prediction model performs forward diffusion and reverse denoising on SE(3) by mapping the relative pose to the tangent space of the Lie algebra se(3), and uses equal-variable noise injection to maintain the invariance of translation and rotation. The reverse denoising uses a distilled K-step solver, where K does not exceed the preset real-time upper limit.

10. A precise docking method for mobile robots based on edge computing according to claim 4, characterized in that, Online reliability calibration is performed on the anisotropic probability tube constructed from the time-varying mean and time-varying covariance: the consistency between the predicted coverage and the measurement residual is statistically analyzed within the sliding window, the scaling factor β≥1 is calculated for each principal axis direction, and the covariance ellipsoid is conservatively expanded according to the scaling factor.

Images