Adaptive fast positioning method and system of robot

Abstract

The application discloses a kind of adaptive fast positioning method and system of robot, belong to mobile robot positioning field, the method includes: obtaining first observation data and second observation data;According to the registration deviation between first observation data and pre-stored environment model determines the sensitivity of the robot pose, and determines a plurality of mutually orthogonal pose directions and the constraint strength index of each pose direction from target constraint measure;For each pose direction, the confidence weight of the first observation data in the direction is contracted by the contraction coefficient determined according to the constraint strength index of the direction, and the confidence weight of the second observation data in the direction is increased correspondingly;According to the confidence weight in each direction, the two types of observation data are fused to obtain the pose of the robot and output.The application retains the existing accuracy of the remaining direction while eliminating the pose slip in the constraint missing direction.

Description

Technical Field

[0001] This invention relates to the field of mobile robot localization, and more particularly to an adaptive rapid localization method and system for robots. Background Technology

[0002] In indoor environments such as logistics warehouses, mobile robots typically employ a laser-inertial tight-coupled method for localization. This method involves pre-scanning and mapping the work area to obtain a single-layer occupancy grid map or a single-layer global point cloud map. During runtime, the pre-integrated result of the inertial measurement unit is used as the initial pose value. The current frame's LiDAR point cloud is downsampled using a fixed-size voxel grid and then registered with the global map using a normal distribution transformation or iterative nearest-point variant to obtain the scan-matched pose. The scan-matched pose, wheel odometer increment, and ultra-wideband base station ranging value are then fed into a fixed-length sliding window with an offline-tuned fixed covariance for nonlinear least-squares optimization to output the robot pose. When the pose is lost, a full-map scan matching is performed on the global map using the current frame's point cloud to complete relocalization. However, logistics warehouses commonly employ long, straight aisles and continuous high-bay racking to increase storage density. These aisles can be tens or even hundreds of meters deep, with adjacent aisles featuring identical rack designs. The racks and the goods they support are mostly made of metal, creating strong reflective surfaces for radio frequency signals. Goods on the racks are constantly being retrieved and placed, and pallets are temporarily stacked and moved. Personnel, forklifts, and other robots move through the aisles. Furthermore, the total computing power of vehicle-mounted edge computing platforms is limited and needs to be shared with modules for perception, planning, and communication. Under these conditions, the aforementioned methods reveal several shortcomings.

[0003] Existing logistics robot localization methods suffer from problems such as pose slippage in directions lacking geometric constraints, difficulty in selectively isolating ranging observations contaminated by multipath interference, slow matching and drift caused by map aging, slow recovery after loss and easy misanchoring, disconnect between computing power allocation and working conditions, and non-self-recovery from single-machine failure. Summary of the Invention

[0004] One of the objectives of this invention is to provide an adaptive and rapid localization method for robots to solve the problems in the prior art, such as pose slippage in directions with missing geometric constraints, difficulty in selective isolation of contaminated observations, slow matching and drift caused by map aging, slow recovery after loss and easy misanchoring, disconnect between computing power allocation and working conditions, and non-self-recovery of single-machine failure.

[0005] This invention is achieved through the following technical solution: an adaptive rapid localization method for a robot, comprising the following steps: acquiring first observation data and second observation data, wherein the first observation data is structural observation data of the environment in which the robot is located, and the acquisition principle of the second observation data is different from that of the first observation data; determining a target constraint metric based on the sensitivity of the robot's pose to the registration deviation between the first observation data and a pre-stored environmental model, and determining multiple mutually orthogonal pose directions and a constraint strength index for each pose direction by the target constraint metric, wherein the constraint strength index indicates the constraint capability of the first observation data on the corresponding pose direction; for each pose direction, determining a contraction coefficient according to the constraint strength index of that pose direction, and contracting the confidence weight of the first observation data in that pose direction by the contraction coefficient, and correspondingly increasing the confidence weight of the second observation data in that pose direction, wherein the contraction coefficient decreases as the constraint strength index decreases; fusing the first observation data and the second observation data according to the confidence weights in each pose direction to obtain and output the robot's pose.

[0006] Furthermore, the first observation data includes lidar point cloud data; the second observation data includes at least one of inertial measurement data, wheeled odometer data, image data pointing upwards from the robot, and ranging observation data.

[0007] Further, determining the target constraint metric includes: for each observation element in the first observation data, determining the registration deviation between the observation element and the corresponding part in the environment model, and determining the sensitivity vector of the registration deviation to each component of the robot pose; weighting the outer product of the sensitivity vector according to the reciprocal of the observation noise of the observation element, and accumulating the weighted results of all observation elements to obtain the target constraint metric; the observation noise increases with the increase of the distance between the observation element and the robot, and increases with the increase of the angle between the orientation of the environmental structure at the observation element and the observation line of sight of the observation element; the constraint strength index is the ratio of the metric value of the target constraint metric in the pose direction to the largest metric value in all pose directions.

[0008] Furthermore, before determining the multiple mutually orthogonal pose directions by the target constraint metric, the method further includes: determining a scale quantity based on the distance of each observation element in the first observation data relative to the robot; converting the angular component of the target constraint metric related to the pose into a component with the same dimensions as the length component of the pose using the scale quantity to obtain a normalized target constraint metric; the multiple mutually orthogonal pose directions are determined by the normalized target constraint metric.

[0009] Furthermore, the adaptive fast localization method further includes: determining the pose direction where the constraint strength index is lower than a first threshold as a constraint-deficient direction; applying a damping amount to each pose direction during the fusion, wherein the damping amount increases as the constraint strength index of the pose direction decreases; and determining a structural orientation distribution index based on the degree of concentration of the orientation of the environmental structure at each observation element in the first observation data; the determination of the pose direction where the constraint strength index is lower than the first threshold as a constraint-deficient direction includes: determining the pose direction as a constraint-deficient direction only when the constraint strength index of the pose direction is lower than the first threshold and the structural orientation distribution index is lower than a second threshold.

[0010] Further, the first threshold includes an entry threshold and a release threshold that are not equal, wherein the release threshold is greater than the entry threshold; determining the pose direction where the constraint strength index is lower than the first threshold as a constraint-deficient direction includes: determining the pose direction as a constraint-deficient direction when the constraint strength index of the pose direction is lower than the entry threshold; releasing the determination of the constraint-deficient direction when the constraint strength index of the pose direction is higher than the release threshold; and maintaining the pose when the constraint strength index of the pose direction is between the entry threshold and the release threshold. The direction determination result at the previous moment; the fusion includes: for pose directions that are not the direction of missing constraints, calculating the pose components in the pose direction from the first observation data and the second observation data according to the confidence weight in the pose direction; for the direction of missing constraints, using the component of the prior pose increment in the direction of missing constraints as the pose component in the direction of missing constraints, the prior pose increment being calculated from observation data that does not depend on the environment model; and synthesizing the pose components in each of the pose directions according to the plurality of mutually orthogonal pose directions to obtain the pose of the robot.

[0011] Further, the contraction coefficient and the constraint strength index have a monotonic mapping relationship without trainable parameters. This monotonic mapping relationship ensures that: the contraction coefficient approaches zero when the constraint strength index approaches zero; the contraction coefficient approaches one when the constraint strength index is greater than a half-point threshold by a set multiple; and the contraction coefficient is equal to half when the constraint strength index is equal to the half-point threshold. Before contracting the confidence weight of the first observation data in the pose direction with the contraction coefficient, the method further includes: replacing the contraction coefficient used in the current positioning cycle with a weighted sum of the contraction coefficient determined in the current positioning cycle and the contraction coefficient used in the previous positioning cycle. This increases the confidence weight of the second observation data in the pose direction accordingly, including: in each pose direction, keeping the sum of the confidence weights of the first observation data and the second observation data constant, so that the share of the confidence weight of the first observation data that is contracted is transferred to the second observation data according to the ratio of the constraint capability of the second observation data in the pose direction.

[0012] Further, the second observation data includes: ranging observation data, which consists of the distance between a reference point with a known location and the robot; the adaptive fast localization method further includes:

[0013] The predicted distance between the robot and the reference point is calculated based on the prior pose increment, and the difference between the ranging observation data and the predicted distance is determined as the deviation. The prior pose increment is calculated from observation data that does not depend on the environment model. When the deviation is positive and greater than a first threshold, the confidence weight of the ranging observation data is reset to zero. When the deviation is negative and its absolute value is greater than a second threshold, the confidence weight of the ranging observation data is reset to zero. The first threshold is less than the second threshold. When the deviation is positive, greater than a set transition lower bound, and not greater than the first threshold, the confidence weight of the ranging observation data is determined monotonically decreasing according to the absolute value of the deviation.

[0014] Furthermore, the adaptive fast positioning method further includes: statistically analyzing the proportion of positive deviations of the same reference point within a set time window; when the proportion is higher than a set offset determination proportion, reducing the confidence weight of the reference point in the ranging observation data of subsequent positioning cycles according to the degree to which the proportion exceeds the offset determination proportion; and when the proportion of non-positive deviations of reference points with reduced confidence weights within a set recovery observation window is higher than a set recovery proportion, resolving the reduction of the confidence weight of the reference point.

[0015] Furthermore, the environment model consists of multiple spatial units, each spatial unit corresponding to a survival index and a change frequency index. The survival index indicates the confidence level of the spatial unit being occupied, and the change frequency index indicates the frequency of changes in the occupancy status of the spatial unit. The environment model includes a first subset, a second subset, and a third subset. The first subset is a set of spatial units whose survival index is higher than the upper survival threshold and whose change frequency index is not higher than the change threshold. The second subset is a set of spatial units whose survival index is higher than the upper survival threshold and whose change frequency index is higher than the change threshold. The third subset is a set of spatial units whose survival index is not higher than the lower survival threshold and whose occupancy is observed in the current positioning period. The first observation data used in determining the target constraint metric is selected in the following manner: first, only the observation elements corresponding to the first subset are selected, and the data is determined by... The smallest metric value among the target constraint metrics constituted by the observed elements; when the smallest metric value is lower than the sufficiency lower limit, additional observed elements corresponding to the second subset are selected, and the confidence weight of the observed element is reduced according to the survival probability of the spatial unit corresponding to the observed element. The survival probability is obtained by monotonically mapping the survival index of the spatial unit and takes a value between zero and one; the observed elements corresponding to the third subset are removed before the registration deviation is determined; the reduction is also based on the regional variation index, which is the weighted average of the variation frequency indices of each spatial unit in the neighborhood of the spatial unit, and the larger the regional variation index, the stronger the reduction; and the corresponding part retrieval threshold used by the observed elements corresponding to the first subset when determining the registration deviation is less than the corresponding part retrieval threshold used by the observed elements corresponding to the second subset.

[0016] Furthermore, the adaptive fast localization method further includes determining the processing parameters for subsequent localization cycles in the following order: determining an upper bound for the sampling granularity based on the ratio of the smallest metric value in the target constraint metric formed by the observation elements corresponding to the first subset to the sufficiency lower bound; determining an expected value for the sampling granularity based on the robot's running speed and the distance from the robot to a preset topological node in the environment model; taking the smaller of the upper bound and the expected value as the sampling granularity of the first observation data; determining the registration execution frequency based on the growth rate of the uncertainty metric of the robot pose and the upper limit of the allowable pose error between two registrations; and determining the window length for backend joint optimization based on the remaining amount of computation required according to the sampling granularity and the execution frequency after deducting the computational resource budget.

[0017] Furthermore, for the case of pose loss of the robot, the adaptive fast localization method further includes: extracting structural invariants from the observation data obtained by the robot facing upwards, wherein the structural invariants remain unchanged under rotation and scaling transformations; collecting field strength sequences at equal mileage intervals along the robot's travel distance, normalizing the field strength sequences within a sliding mileage window, and converting them into field strength symbol sequences according to graded thresholds; quantizing the structural invariants and the field strength symbol sequences respectively, concatenating them into a fixed-length spatial identifier code, and searching the spatial identifier code with multiple pre-established reference spatial identifier codes according to the code distance to obtain several candidate topological intervals; and retrieving only the local environment corresponding to the candidate topological intervals. The model is registered, and the registration result is used as the recovered pose after the robot pose is lost. Before using the registration result as the recovered pose after the robot pose is lost, the method further includes: projecting the observation features obtained by the robot facing upwards onto the local environment model corresponding to the candidate topology interval according to the registration result of the candidate topology interval; calculating the proportion of observation features whose distance to the reference features in the local environment model after projection is less than the inlier distance to obtain a consistency index; accepting the registration result of the candidate topology interval only when the consistency index is higher than the acceptance threshold; and expanding the number of candidate topology intervals and re-searching when the consistency index of all candidate topology intervals is not higher than the acceptance threshold.

[0018] Furthermore, for the scenario where the robot is surrounded by obstructions and both the first and second observation data simultaneously lose their constraint capabilities, the method further includes: broadcasting a distress message to a second robot within the communication range when the growth rate of the robot's pose uncertainty measure within a set time period exceeds a divergence threshold, or when the component of the robot's pose uncertainty measure in the pose direction with the lowest constraint strength index exceeds a set value; receiving the relative pose relationship calculated and sent by the second robot based on its observation data matching the contour description; combining the pose of the second robot with the relative pose relationship to obtain the robot's pose prior; and combining the uncertainty measure of the second robot's pose with the uncertainty measure of the relative pose relationship according to the Lie group adjoint transform to form the uncertainty measure of the pose prior; and adding the pose prior and its uncertainty measure as prior constraints to the robot's back-end joint optimization and resolving the problem.

[0019] Furthermore, the robot and the second robot each maintain a source identifier set, which records the identifiers of external information providers relied upon by the robot for pose estimation; the step of adding the pose prior and its uncertainty metric as prior constraints to the robot's backend joint optimization includes: adding the pose prior as a prior constraint and resolving it only when the intersection of the source identifier set of the robot and the source identifier set of the second robot is empty; otherwise, discarding the pose prior; after the pose prior is added, merging the source identifier set of the second robot and the identifier of the second robot into the source identifier set of the robot; and clearing the source identifier set of the robot after the robot obtains a pose with a higher confidence level than the set based on its own observation data.

[0020] Furthermore, before combining the pose of the second robot with the relative pose relationship, the method further includes: extrapolating the pose of the second robot at the time of transmission to the time when the robot receives the relative pose relationship using the pose increment calculated from the observation data of the second robot independent of the environment model; and adding the uncertainty introduced by the extrapolation to the uncertainty of the pose prior.

[0021] Another aspect of the present invention provides an adaptive rapid localization system for a robot, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement any of the adaptive rapid localization methods for a robot described above.

[0022] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0023] 1. This invention constructs a target constraint metric based on the sensitivity of registration deviation to pose and solves for mutually orthogonal pose directions and constraint strength indices in each direction. This separates the loss of geometric constraints in a certain degree of freedom from the overall unreliability of a certain observation source, allowing confidence adjustment to be performed component-wise along the direction. Furthermore, by applying a damping amount that increases with the degree of degradation in the direction of constraint loss and replacing the solution in that direction with a component of the prior pose increment, the propagation path of observation noise along that direction is mathematically cut off.

[0024] 2. This invention introduces structural orientation distribution indices with independent causes as a second criterion, and uses entry and release thresholds to form a hysteresis band, which not only distinguishes between the true lack of geometric constraints and the ill-conditioned transient values ​​caused by occlusion, but also suppresses state jitter in the transition region. Furthermore, it utilizes the physical property that the ranging multipath bias can only be positive to construct an asymmetric test with unequal positive and negative thresholds, and supplements it with the determination and release of continuous bias at the reference point level, thereby improving the selectivity of isolation without sacrificing effective observations.

[0025] 3. This invention divides spatial units into a first subset, a second subset, and a third subset using two orthogonal dimensions: a persistence index and a change frequency index. It selects observation elements for registration based on a strategy that prioritizes the first subset, ensuring that the geometric structure upon which the matching depends does not become invalid as the target changes. This also reduces the scale and number of iterations of the nearest neighbor retrieval. Furthermore, it uses a fixed-length spatial identifier code, formed by concatenating the structural invariant observed upwards with the field strength symbol sequence collected along the travel distance. The lost data is recovered through a two-stage process of inter-code distance retrieval and local registration, supplemented by a secondary check using a consistency index. This decouples the recovery time from the map size and eliminates misanchoring caused by geometric repetition. Attached Figure Description

[0026] The accompanying drawings, which are included to provide a further understanding of embodiments of the invention and form part of this application, do not constitute a limitation thereof. In the drawings:

[0027] Figure 1 This is a flowchart of the method provided in Embodiment 1 of the present invention.

[0028] Figure 2 This is a comparison chart of the distribution of constraint strength indicators provided in Embodiment 1 of the present invention.

[0029] Figure 3 This is a comparison chart of the number of false triggers provided in Embodiment 1 of the present invention.

[0030] Figure 4 This is a comparison diagram of the state reversal process provided in Embodiment 1 of the present invention.

[0031] Figure 5 This is a comparison diagram of lateral and axial pose errors provided in Embodiment 1 of the present invention.

[0032] Figure 6 This is the adaptive transition diagram provided in Embodiment 1 of the present invention.

[0033] Figure 7 This is a comparison chart of rejection rates provided in Embodiment 1 of the present invention.

[0034] Figure 8 This is a diagram illustrating the depressurization and recovery process provided in Embodiment 1 of the present invention.

[0035] Figure 9 This is a comparison chart of the cumulative pose slip amount as travel distance provided in Embodiment 1 of the present invention.

[0036] Figure 10 This is a comparison chart of the matching in-point rate provided in Embodiment 1 of the present invention.

[0037] Figure 11This is a comparison chart of the number of participating points and the registration time per frame provided in Embodiment 1 of the present invention.

[0038] Figure 12 This is a comparison chart of processor usage and topology node neighborhood positioning error provided in Embodiment 1 of the present invention.

[0039] Figure 13 This is a comparison chart showing the change in kidnapping recovery time as a function of the environmental model size, provided in Embodiment 1 of the present invention.

[0040] Figure 14 This is a comparison chart of the correct anchoring rates provided in Embodiment 1 of the present invention.

[0041] Figure 15 This is a diagram illustrating the impact of the consistency of uncertainty metrics provided in Embodiment 1 of the present invention. Detailed Implementation

[0042] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. The components of the embodiments of the present invention described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0043] It should be noted that, in this application, multi-source observation data can originate from mechanical or solid-state LiDAR, inertial measurement units, wheel encoders, ultra-wideband or other radio frequency ranging terminals mounted on the mobile robot body, as well as monocular or binocular cameras mounted obliquely upwards or directly upwards. It can also originate from other sensing methods capable of providing equivalent physical quantities. In this application, LiDAR point cloud data includes at least the three-dimensional coordinates and ranging values ​​of each ranging point in the sensor coordinate system; optionally, it may also include auxiliary information such as echo intensity, harness number, or timestamp. Inertial measurement data includes at least three-axis angular velocity and three-axis specific force; wheel odometry data includes at least the angular increments of the left and right drive wheels; radio frequency ranging data includes at least the ranging value and the identifier of the base station emitting the ranging; upward-view image data includes at least a grayscale or color image facing the ceiling. In this application, the extrinsic parameter calibration relationship between the sensors (i.e., the fixed transformation of each sensor coordinate system relative to the body coordinate system) is considered a known quantity that has been calibrated at the time of device manufacturing or initial deployment. In this application, the body coordinate system refers to a right-handed Cartesian coordinate system with the robot's rear axis center as the origin, the forward direction as the X-axis, the leftward direction as the Y-axis, and the vertical upward direction as the Z-axis; the map coordinate system refers to a global reference coordinate system determined during the mapping phase and remaining unchanged thereafter; pose refers to the six-degree-of-freedom rigid body transformation of the body coordinate system relative to the map coordinate system. In this application, a logistics warehouse refers to an indoor work area with continuously arranged high-level shelves, long and straight through-passage work aisles, and frequent goods entry and exit and personnel and vehicle traffic; the method of this application is not limited to this location, and is applicable to any indoor environment with geometric repetition, frequent target changes, or radio frequency multipath characteristics.

[0044] Example 1

[0045] This embodiment discloses an adaptive and rapid localization method for robots. In this embodiment, through the synergistic effect of seven mechanisms, namely, spectral domain perception of scene degradation, directional decoupling and redistribution of multimodal confidence, unilateral isolation of radio frequency multipath, continuous updating and hierarchical matching of static-semi-static-dynamic three-layer spatiotemporal map, rapid anchoring of heterogeneous spatial fingerprint, collaborative scheduling driven by motion and computing power, and distributed correction of multi-machine mutual observation, the robot can continuously obtain stable, fast pose estimation under limited edge computing power conditions in indoor environments with highly repetitive geometric features, frequent changes in scene targets, and severe multipath contamination of radio frequency observation. The robot can also recover pose estimation after loss. Figure 1 The overall flowchart of this embodiment is shown. As can be seen from the figure, this embodiment includes the following steps:

[0046] Step 1: Acquire multi-source observation data of the robot during the current positioning cycle.

[0047] Multi-source observation data refers to the collection of observations gathered by several sensors mounted on the robot body, each with independent physical principles, within the same positioning cycle. In this embodiment, multi-source observation data includes five categories: lidar point cloud data, inertial measurement data, wheeled odometer data, radio frequency ranging data, and upward-view image data.

[0048] LiDAR point cloud data refers to the set of three-dimensional points obtained by LiDAR ranging the surrounding environment within one scanning cycle. It is used as the primary observation source because laser ranging is not affected by lighting conditions, its accuracy is stable at the centimeter level, and it can directly provide the geometric structure of the environment, making it the most constrained observation method for indoor positioning.

[0049] Inertial measurement data refers to the triaxial angular velocity and triaxial force sequence output by the inertial measurement unit at a sampling rate much higher than that of the laser scanning frequency. This data is introduced because it has extremely high short-time recursive accuracy and is completely independent of the external environment. It can maintain the continuity of pose between two laser scans and provide a reliable short-time reference benchmark for other observations.

[0050] Wheel odometry data refers to the displacement and heading increments of the aircraft obtained by solving the kinematic model from the rotational increment of the drive wheel encoder. This data is introduced because it can provide high-precision longitudinal displacement scales under flat, slip-free conditions, which can compensate for the scale drift of the accelerometer's second integration under low dynamic conditions.

[0051] Radio frequency ranging data refers to the distance values ​​measured between a vehicle-mounted radio frequency terminal and several known coordinate base stations deployed in a warehouse. This data is introduced because it provides absolute position constraints and is the only low-cost means to suppress long-term cumulative drift.

[0052] Upward-view image data refers to images captured by cameras mounted facing the ceiling. This observation path is specifically introduced because in logistics warehouses, the scene at eye level on the ground is both highly repetitive and highly dynamic, while the fire pipes, trusses, and lighting arrays on the ceiling remain unchanged for years after the factory is built, and their main orientation usually extends longitudinally along the aisles. This complements the constraint direction that LiDAR lacks in long, straight aisles.

[0053] In this embodiment, before entering subsequent processing, the observation data from each path are unified to the same time reference by means of hardware synchronization or software interpolation; the lidar point cloud data is further compensated for motion distortion by recursively calculating the pose from the inertial measurement data, that is, each point in a scanning cycle is projected onto the body coordinate system at the end of the cycle according to its own timestamp, so as to eliminate the point cloud stretching caused by the robot's movement.

[0054] It should be noted that the five types of observation data acquired in this step are not all indispensable. When the robot is not equipped with a radio frequency terminal or the area has no base station coverage, the radio frequency ranging channel can be omitted, and the rest of the method will not be affected. When the robot is not equipped with an upward-view camera, the constraint in the degradation direction will be mainly borne by inertia and wheel odometry, which is less effective than when a camera is equipped, but the logic of the method still holds. In other words, the specific composition of multi-source observation data is open, and its essential requirement is only the existence of two or more observation sources with independent physical principles and complementary constraint directions in pose space.

[0055] Step 2: Register the lidar point cloud data with the pre-stored three-layer spatiotemporal map, construct a six-dimensional observation information matrix, and perform dimensionally normalized spectral decomposition on it to extract the current orientation basis, observability sequence, and degradation direction.

[0056] The six-dimensional observation information matrix refers to a sixth-order symmetric positive semi-definite matrix obtained by weighting and summing the sensitivity of the residuals of all point cloud points involved in the registration to the six-dimensional pose increment according to the inverse of the observation noise. The reason for explicitly constructing this matrix is ​​that it is simultaneously the approximate Hessian matrix of the registration optimization problem near the convergence point and the Fisher information matrix carried by this batch of observations. Curvature and information are two sides of the same coin; the flatter the curvature in a certain direction, the less information is carried in that direction, and the less discernible the pose is in that direction.

[0057] A directional basis is a set of six mutually orthogonal eigenvectors obtained by eigenvalue decomposition of the matrix after dimensional normalization. This basis is introduced because the six degrees of freedom of the pose are coupled to each other in the original coordinate system; only with this basis can the question of which degree of freedom is unconstrained be definitively answered.

[0058] Observability refers to the ratio of an eigenvalue to the largest eigenvalue in a certain direction, and it is a value with a range of [value missing]. The dimensionless index; degradation refers to the complement of observability. Degradation direction refers to the direction in which observability is below a set threshold and simultaneously satisfies the condition of geometric anisotropy.

[0059] In this embodiment, in order for the robot to determine which degree of freedom has lost its geometric constraint in each frame without relying on any external truth value, the specific steps for constructing the observation information matrix and extracting the degradation direction may include the following:

[0060] Sub-step 1: Construct point-to-surface residuals and point-level noise with incident angle correction.

[0061] Point-to-surface residuals refer to the signed distance from a point in the current frame to the fitted plane of its corresponding neighborhood in the map after transforming it to the map coordinate system according to the estimated pose. The reason for using point-to-surface residuals instead of point-to-point residuals is that the surfaces of shelving facades, floors, and load-bearing columns constitute the vast majority of the warehouse. Point-to-surface residuals can accurately reflect the geometric fact that points can slide freely within a surface without error, thus avoiding the introduction of false constraints in the planar region by point-to-point residuals.

[0062] In this embodiment, to prevent low-quality ranging points hitting the shelf facade at extremely small grazing angles from contaminating the information matrix with false high confidence and thus distorting the degradation judgment itself, the specific implementation of point-to-surface residuals and point-level noise is as follows:

[0063] For each point after downsampling in the current frame, search for its nearest neighbors in the map and fit a local plane, then take the unit normal vector and center point of the plane;

[0064] The residual is the projection of the difference between the coordinates of the point after the transformation according to the current pose estimation and the center point of the plane onto the normal vector;

[0065] The noise variance at a point is the sum of the basic noise variance and the variance that increases with the square of the distance measurement and amplifies with the inverse of the cosine of the incident angle. A lower limit is applied to the cosine of the incident angle to prevent variance divergence.

[0066] For example, in this embodiment, the point-to-surface residual and the point-level noise can be calculated using the following formula:

[0067]

[0068]

[0069] in, The desired six-dimensional pose increment; Its translation component; Let it be its rotation vector component; This is the rotation matrix corresponding to the rotation vector; For the current frame number The coordinates of a point in the body coordinate system; and These are the coordinates of the center point of the neighborhood corresponding to the point on the map and the unit normal vector, respectively. This is the distance measured at that point; Let this be the unit vector of the laser line of sight at that point; That is, the cosine of the angle of incidence; The standard deviation of the baseline noise of the ranging system can be taken as 0.02 meters in this embodiment; The distance-proportional noise figure can be taken as two per thousand. This is the cutoff value for the grazing angle, which can be taken as 80 degrees.

[0070] Understandably, the denominator in the noise model is a customized design for long, straight aisle warehouse scenarios. When the laser beam is directly incident on the shelf facade, the cosine of the incident angle is close to 1, and the noise at that point degenerates into conventional distance-dependent noise. When the laser beam passes almost parallel to the facade, the cosine of the incident angle approaches zero, and the noise is amplified several times or even tens of times, thus reducing the contribution of that point to the information matrix. In long, straight aisles, it is precisely the points that sweep across distant shelves with extremely small grazing angles that are numerically dominant. Without correction, these points will cause the information matrix to exhibit false constraint strength along the aisle axis, masking the true degradation.

[0071] Sub-step 2: Accumulate the pose Jacobian outer product and perform dimensional normalized spectral decomposition.

[0072] The pose Jacobian refers to the row vector formed by the first-order partial derivatives of the point-plane residuals with respect to the six-dimensional pose increment, where the first three components are the sensitivity to translation and the last three components are the sensitivity to rotation.

[0073] The characteristic radius refers to the mean square radius of the point set involved in the registration relative to the origin of the body coordinate system. This scale is introduced because three of the six degrees of freedom in pose are translation (in meters) and three are rotation (dimensionless). The information matrix formed by these three components has different dimensions for different sub-blocks, making direct comparison of eigenvalues ​​impossible. Therefore, the statement about the direction corresponding to the smallest eigenvalue becomes meaningless. The physical meaning of the characteristic radius is precisely the distance traveled by a typical point on the point cloud when rotating a unit angle around the body origin. Using it to convert the rotational degrees of freedom into equivalent arc-length displacement is the most natural choice to bring all six degrees of freedom into the same dimension.

[0074] For example, in this embodiment, the observation information matrix and its dimensionally normalized spectral decomposition can be calculated as follows: First, sum the Jacobian outer products of each point:

[0075]

[0076]

[0077] Secondly, the characteristic radius is calculated and the scaling matrix is ​​constructed, and the information matrix is ​​normalized in terms of dimensions:

[0078]

[0079]

[0080] Finally, a symmetric eigenvalue decomposition is performed on the normalized matrix to obtain the directional basis and observability sequence:

[0081]

[0082]

[0083] in, The number of points participating in the registration; The cross product of three-dimensional vectors; The characteristic radius; The scaling matrix; The information matrix after dimension normalization has six diagonal blocks with uniform length square reciprocal dimensions. For orthogonal directions in normalized coordinates; The eigenvalues ​​are arranged in descending order; For the first Observability in each direction; This represents the degree of degradation in that direction; The sign is the transpose of the matrix; when returning to the original pose coordinates, the direction basis is taken as... .

[0084] Understandably, after this normalization, Only then does it truly become a dimensionless index that can be compared across degrees of freedom: A value close to 1 means that the constraint strength in that direction is comparable to that in the strongest direction. Approaching zero means that there is almost no constraint in that direction. In a typical long straight corridor in a warehouse, there is usually exactly one direction where the observability collapses to the order of one-thousandth, while the observability of the other five directions is all above one-tenth. This unique collapse is the fingerprint of the corridor effect in the spectral domain. Figure 2 This diagram shows a comparison of the constraint strength index distribution in the open area and the six pose directions in the long straight channel in this embodiment; from Figure 2 As can be seen, under the condition of a long straight channel, the minimum constraint strength index collapsed from 0.16 in the open area to 0.0012, a decrease of 133.3 times, while the constraint strength index in other directions remained comparable to that in the open area.

[0085] Sub-step 3: Cross-validate using normal tensors and determine the degradation direction using double-threshold hysteresis.

[0086] Here, the normal tensor refers to the second-order moment tensor of the unit normal vectors of the registration point set; the geometric anisotropy index refers to the ratio of the minimum eigenvalue to the maximum eigenvalue of this tensor. The reason for introducing this second criterion, independent of the optimization structure, is that when a forklift happens to drive past the robot's side and front, blocking most of the field of view, the number of points involved in registration will drop sharply within one or two frames, the information matrix will become ill-conditioned, and the minimum observability will also collapse briefly. However, this is not a true geometric degradation, and everything will recover after the occlusion ends. If weight redistribution is triggered based on this, it will introduce unnecessary pose jumps. Figure 3 This embodiment shows a comparison of the number of false triggers for the ill-conditioned single-criteria and dual-criteria methods for transient numerical transients caused by forklift obstruction; from Figure 3 As can be seen, in the transient interval caused by occlusion, there were 41 false triggers based solely on the constraint strength index, while the number of false triggers based solely on the dual criteria of the superimposed structure orientation distribution index (whose minimum value in this interval is still 0.377, which is much higher than the second threshold) was 0.

[0087] In this embodiment, in order to distinguish between the actual geometric defects of the corridor and the transient ill-conditioned values ​​caused by occlusion and sudden drop in points, and to avoid high-frequency jitter of the gating coefficient caused by the robot repeatedly crossing the decision threshold near the passage entrance, the determination of the degradation direction adopts a combination of dual-source crossover and dual-threshold hysteresis. The specific implementation method is as follows:

[0088] The unit normal vectors of the registration point set are accumulated externally to obtain the normal tensor, and the ratio of its minimum to maximum eigenvalues ​​is taken as the geometric anisotropy index.

[0089] When the observability of a certain direction is lower than the entry threshold and the geometric anisotropy index is also lower than the anisotropy threshold, that direction is set to a degenerate state.

[0090] When the observability in this direction is higher than the release threshold, it is removed from the degenerate state;

[0091] When the observability falls between the entry threshold and the exit threshold, the decision made at the previous time step remains unchanged.

[0092] For example, in this embodiment, the normal tensor, geometric anisotropy index, and hysteresis determination can be calculated using the following formula:

[0093]

[0094]

[0095] in, For normal tensors; and These are its minimum and maximum eigenvalues, respectively; The geometric anisotropy index has a range of values. When the normal vector is uniformly distributed in all directions, it approaches 1; when it is highly concentrated in a single direction, it approaches zero. The threshold for triggering the degradation state can be set to 0.01 in this embodiment; The threshold for exiting the degenerate state can be set to 0.05, and the interval between the two values ​​constitutes the hysteresis band. The anisotropy threshold can be set to 0.08. This represents the result of the direction determination at the previous moment.

[0096] It's important to note that the warehouse passageway is a transition zone between the geometry of a corridor and the geometry of an open space, where observability inevitably slowly crosses the threshold. If a single threshold is used, the robot will experience dozens of degenerate-to-non-degenerate state flips within one or two seconds of passing through the intersection. Each flip causes a step change in the observation weights, resulting in observable jitter in the pose output, which can even trigger oscillations in the downstream path tracking controller in severe cases. The hysteresis band reduces this number of flips to at most one, at the cost of only a slight delay in state transition time. In this scenario, this delay is far more acceptable than the jitter. Figure 4 This diagram illustrates a comparison of the state reversal process between single-threshold and dual-threshold hysteresis determinations in the channel transition zone of this embodiment; from Figure 4 As can be seen, when the robot passes through the transition zone at the channel entrance, the single threshold judgment resulted in 73 constraint-missing state flips, while the double threshold hysteresis judgment only resulted in 1 state switch throughout the entire process.

[0097] Step 3: Project the information matrices of each observation source onto a unified direction basis, and redistribute the multi-source observation weights component by component according to direction.

[0098] Here, directional information refers to the Rayleigh quotient of the information matrix of a certain observation source in a specific direction of the directional basis; its physical meaning is the constraint ability of that observation source on that degree of freedom. Multi-source observation weights refer to the normalized relative confidence values ​​assigned to each observation source in each direction.

[0099] The reason for assigning weights component-by-component according to direction, rather than assigning a scalar confidence level to each observation as in traditional methods, is that losing a constraint on a degree of freedom and an unreliable sensor are two completely different things. In long straight channels, the constraints of laser on lateral translation and heading angle not only do not deteriorate, but are actually stronger than in open areas due to the presence of a large number of coplanar points; if the laser weight is reduced overall due to the degradation of the channel axis, it is equivalent to discarding these high-quality constraints as well, resulting in a significant deterioration in lateral accuracy, but the lateral direction is a critical direction to prevent the robot from rubbing against the shelves. Figure 5 This diagram shows a comparison of the overall weight reduction scheme and the orientation decoupling scheme in this embodiment regarding lateral and axial pose errors; from Figure 5 As can be seen, the root mean square error of the lateral pose of the directional decoupling scheme is 0.0461 meters, which is 58.7% lower than the 0.1118 meters of the overall weighted scheme, while its root mean square error of axial pose is only 1.042 times that of the overall weighted scheme.

[0100] In this embodiment, in order to automatically transfer the reduced laser discourse power to other observation sources that still have constraint capabilities in that direction while only shrinking the laser discourse power in the degradation direction, and at the same time fully preserving the laser constraints in other directions, the multi-source observation weighting can specifically include the following sub-steps:

[0101] Sub-step 1: Construct the information matrix of each observation source.

[0102] In this embodiment, the information matrices of each observation source are constructed according to their respective physical principles:

[0103] The information matrix of one LiDAR path is the one constructed in step 2. ;

[0104] The information matrix of the inertial path is the inverse of the covariance of the pre-integrated residual between two positioning cycles. Its characteristic is that it provides moderately strong constraints in all six degrees of freedom, which decrease as the integration time increases.

[0105] The information matrix of a wheel odometer is constructed from the inverse of the noise variance of its longitudinal displacement increment and heading increment, and is transformed accordingly according to the rotation from the body coordinate system to the map coordinate system. Its characteristics are that it has strong constraints on longitudinal translation and heading angle, and almost no constraints on lateral translation and vertical direction.

[0106] The information matrix of the upward-view visual path is obtained by weighting and summing the reprojection residual of the ceiling line features with the Jacobian of the pose according to the inverse of its pixel noise. Its characteristics are complementary to those of the laser. The fire pipes and lights on the ceiling are arranged longitudinally along the channel, and the displacement of the endpoints of their line features in the image is highly sensitive to the axial translation of the channel.

[0107] The information matrix of the radio frequency ranging channel is obtained by weighted summation of the outer product of the unit vectors in the line-of-sight direction of each base station according to the reciprocal of their ranging noise, and its constraint direction is determined by the spatial layout of the base stations.

[0108] Before being projected, all five information matrices are first normalized to the same scale as those in step 2, i.e., taken as... .

[0109] It should be noted that the five information matrices must be normalized using the same scale matrix. If each matrix selects its own scale, their directional information will not be on the same dimension, their ratios will no longer have a meaning of relative constraint, and subsequent normalization allocation will lose its basis.

[0110] Sub-step 2: Project the direction information and redistribute the weights according to the deterministic gating function.

[0111] Among them, the degradation gating coefficient refers to a value range of A scalar function is used to shrink the directional information of the laser in that direction based on the observability in that direction. The rational form of the gating function is used because it approaches zero when the observability approaches zero, approaches one when the observability is much greater than the gating threshold, and is monotonically increasing and differentiable throughout the entire domain. It also contains no learnable parameters that need to be determined by data training, and both of its constants have direct physical interpretations and can be directly tuned by sensor specifications and scene geometry. Compared to weighted adjustment networks that require training, this form of behavior is analytically predictable across the entire domain, a decisive advantage for industrial robots that must pass safety certifications.

[0112] For example, in this embodiment, the direction information, gating coefficient, and direction weight can be calculated as follows: First, project each source information matrix onto the direction basis:

[0113]

[0114] Secondly, the gating coefficients are calculated based on observability and then subjected to first-order smoothing.

[0115]

[0116] Finally, normalize all observation sources within each direction to obtain the direction weights:

[0117]

[0118] in, For the first The normalized information matrix of the road observation sources, with subscripts The following are represented in sequence: lidar point and area observation, inertial pre-integration, wheeled odometer, upward-view visual observation, and radio frequency ranging observation; For this observation source in the th directional information in each direction; The original gating coefficients calculated for the current frame; The threshold value is the half-point threshold for gating. Its physical meaning is that when the observability is exactly equal to this value, the weight of the laser in that direction is halved. In this embodiment, it can be taken as 0.03. The steepness exponent is a positive integer, and can be 2. The coefficient for the first-order preserve filter can be 0.3; The indicator function is used to ensure that the gating term is taken only for the laser path. Take 1 for all other paths; The final directional weights are summed to 1 for all observed sources in each direction.

[0119] Understandably, in a long, straight channel, the channel axis corresponds to the 6th direction. While the original directional information of the laser in this direction is small, it is not zero, while the directional information of inertia and the upward-viewing perspective is relatively considerable. When the observability of this direction collapses to the order of one-thousandth, the gating coefficient approaches zero, the laser term in the numerator is eliminated, and the denominator decreases simultaneously. Thus, the weights of the upward-viewing perspective and inertia automatically rise to a proportion close to 1, and the top observation takes over the constraint of the channel axis without any explicit instructions. At the same time, in the direction corresponding to the lateral direction, because the observability is close to 1 and the gating coefficient is close to 1, the laser weight still dominates. Figure 6 This embodiment illustrates the adaptive transition diagram of the confidence weights of each observation source in the direction of constraint loss during the entry into the long straight channel; from Figure 6 As can be seen, after the robot enters the long straight channel, the confidence weight of the first observation data in the direction of missing constraints decreases from 0.909 to 0.043, while the sum of the confidence weights of the other observation sources in this direction increases to 0.957. Throughout the process, the sum of the confidence weights of the five observation sources in the same direction remains strictly one.

[0120] It should be noted that dynamically increasing the confidence level of other sensors in this dimension does not require any explicit boosting rules in this method; it is a necessary consequence of the normalization constraints. This makes the model structurally extremely simple and avoids the additional parameters that are difficult to tune in explicit boosting rules. If normalization is ignored and independent boosting coefficients are used instead, the sum of weights in each direction is no longer conserved, and the overall scale of the optimization problem will drift with the degenerate state, thus making a series of downstream parameters such as the damping coefficient and convergence criterion incomparable.

[0121] Step 4: Using the predicted ranging based on inertial recursion as a benchmark, perform asymmetric consistency checks and adaptive isolation on the radio frequency ranging data based on multipath unilaterality.

[0122] In this context, "news" refers to the difference between the measured ranging value and the predicted ranging value calculated from the inertial recursive pose. "Asymmetric threshold" refers to the test threshold that takes different values ​​for positive and negative news. "Persistent obstruction state" refers to a state where a base station's news symbol remains positive for a continuous period of time, thus being determined as having a long-term blocked direct path.

[0123] The reason for using an asymmetric threshold is that the physical causes of multipath propagation dictate the unilateral nature of its bias: when a direct path is interrupted by a metal pillar, the receiver can only lock onto a longer path reflected from the roof or adjacent shelves. Therefore, the ranging error caused by multipath propagation can only be positive. Using a symmetric threshold to test a unilaterally distributed error is equivalent to wasting half of the threshold budget in a direction that will never occur. Widening the threshold absorbs the positive bias into the optimization, while tightening the threshold misjudges a large number of normal fluctuations as abnormal. Figure 7 This embodiment shows a comparison of the rejection rates of symmetric and asymmetric thresholds with positive multipath bias at the same false rejection rate; from Figure 7 As can be seen, under the comparable conditions of 5.14% and 4.73% false rejection rates for multipath observations, the asymmetric threshold achieves a rejection rate of 90.02% for multipath observations, which is 24.59 percentage points higher than the symmetric threshold's 65.42%. This demonstrates that the asymmetric threshold can significantly improve the selectivity of isolation without sacrificing effective observations.

[0124] In this embodiment, in order to maximize the rejection rate of forward offset ranging while maintaining the same false rejection rate, and to enable base stations judged as obstructed to be reactivated after the robot leaves the obstruction area, the specific implementation method of radio frequency ranging verification and isolation is as follows:

[0125] The predicted position at the current time is obtained by superimposing the pose at the previous ranging time with the inertial pre-integration increment. Based on this, the predicted ranging value to each base station is calculated, and the difference between the measured value and the predicted value is taken as the information.

[0126] The theoretical standard deviation of the innovation is obtained by combining the projection of the inertial recursive position covariance onto the line-of-sight direction of the base station with the ranging noise variance;

[0127] The new information is assigned a weighting factor according to the four-segment rule: those exceeding the positive threshold are set to zero, those falling between the lower boundary of the softening zone and the positive threshold are decayed according to the inverse law of the square of the residual, those falling within the softening zone are set to 1, and those below the negative threshold are set to zero.

[0128] The proportion of positive news for each base station in the most recent time window is calculated. When the proportion exceeds the occlusion threshold, the weight of the base station is reduced as a whole according to the degree of excess.

[0129] The percentage of non-positive news from suppressed base stations within the recovery observation window is counted, and suppression is lifted when the percentage exceeds the recovery threshold.

[0130] For example, in this embodiment, the innovation and asymmetric threshold test can be calculated using the following formula:

[0131]

[0132]

[0133]

[0134] in, For the first The known coordinates of the radio frequency base stations; Position of the robot; For pose composite operators; The pose increment given by inertial pre-integration between two ranging moments; This is the actual measured distance value; For new information; The theoretical standard deviation of the new information; and These are the positive and negative threshold coefficients, respectively, satisfying... In this embodiment, the values ​​can be 2.0 and 3.5 respectively; This is the lower limit coefficient of the softening zone, which can be taken as 1.0; This is the weighting factor for this ranging observation.

[0135] Understandably, the middle two segments of the four-segment function constitute a smooth transition region: observations whose information falls between the lower bound of the softening region and the positive threshold are neither fully trusted nor completely discarded, but their weights are decayed according to the inverse law of the squared residual. This design avoids the weight jumps caused by hard thresholds near the threshold. If a hard threshold is used, an observation that fluctuates slightly around the threshold will repeatedly switch between being fully trusted and completely discarded, and its impact is just as harmful as the gating jitter described in step 2.

[0136] For example, in this embodiment, the persistent occlusion determination and recovery at the base station level can be calculated using the following formula:

[0137]

[0138]

[0139]

[0140] in, This is a sequence of recent information about the base station within a given timeframe. It is an indicator function; The percentage of positive new interest; To determine the threshold for the proportion of continuous occlusion, a value of 0.8 can be used in this embodiment; The overall weighting strength is defined, and its value range is [value range missing]. 0.9 can be taken; To restore the viewing window; The threshold for the proportion of non-positive new information required to relieve the pressure can be set at 0.6.

[0141] It's important to note that successive thresholds alone, without base station-level judgment, are insufficient. When a base station is continuously blocked by tall shelving, each ranging measurement carries a similar positive bias. While the innovation is consistently positive, its amplitude may never exceed the single-step threshold. Consequently, these systematic biases are allowed to pass one by one, continuously polluting the optimization results. The statistic of the positive innovation ratio can identify systematic biases before a single innovation exceeds the limit, serving as a necessary supplement to successive thresholds. Conversely, if the release condition is ignored, the algorithm exhibits a more subtle degradation over long-term operation: whenever the robot passes through a densely shelved area, one or two base stations will be judged as blocked and weighted. Due to the lack of a recovery mechanism, the weighted state persists, and after several hours, almost all base stations are in a weighted state, rendering the radio frequency constraints ineffective, while no anomalies are visible in the system logs. The recovery criterion transforms the weighting from a one-time judgment into a reversible state machine, a necessary condition for the long-term stable operation of this stage. Figure 8 This diagram illustrates the process of suppressing and recovering the ranging observation confidence weight when the reference point is continuously occluded in this embodiment; from Figure 8 As can be seen, within 1.8 seconds after the reference point enters the continuous occlusion, the confidence weight of its ranging observation is reduced to 0.1, while within 0.5 seconds after the robot leaves the occlusion area, the weight recovers to 1.0.

[0142] Step 5: Apply adaptive damping under the orientation basis and apply prior substitution to the degradation direction, solve for the pose increment and output the positioning result.

[0143] Here, adaptive damping refers to a direction-selective regularization term whose strength increases with the degree of degradation in that direction. Prior substitution refers to directly replacing the component solved by observation with the projection of the prior pose increment in that direction for directions already determined to be degenerate. Prior pose increment refers to the pose increment obtained recursively from the body motion sensor, without relying on any external environmental observations.

[0144] The reason why damping and replacement are needed in addition to direction weights is that direction weights address the question of which system to trust, while ill-conditioned linear systems also have a purely numerical problem: even if the laser weight is reduced to an extremely low value in the degenerate direction, as long as that direction still participates in solving the linear equations, its extremely small eigenvalues ​​will still be amplified into a very large pose increment during inversion. This is an inherent property of ill-conditioned systems and is unrelated to the level of weights.

[0145] In this embodiment, in order to maintain the quadratic convergence rate of the Gauss-Newton method in the benign direction while completely cutting off the propagation path of observation noise in the degenerate direction, the solution of the pose increment may specifically include the following sub-steps:

[0146] Sub-step 1: Synthesize prior pose increments.

[0147] In this embodiment, the prior pose increment is mainly based on inertial pre-integration and the longitudinal increment of wheel odometry is used as the scale constraint. The two are weighted and synthesized according to the reciprocal of their respective noise variances. When the robot is turning in place or detects wheel slippage (manifested as a continuous inconsistency between wheel speed integral and inertial integral in the longitudinal direction), it automatically degenerates to using only inertial pre-integration.

[0148] It should be noted that the reliability of the prior pose increment is a prerequisite for the validity of the prior replacement in the degradation direction. The reason for completely abandoning the observation and taking the prior in the degradation direction is that, within the time scale of traveling through a passage of tens of meters (usually tens of seconds), the longitudinal recursive error of inertia and wheeled odometers is a slowly accumulating bounded quantity, the magnitude of which is usually a few thousandths of the travel distance; while the observation noise in the degradation direction is unbounded. Figure 9 This embodiment shows a cumulative comparison of pose slip along the channel axis as a function of travel distance within a long straight channel; from Figure 9 As can be seen, when the travel in the 60-meter-long straight channel ends, the fixed-weight scheme accumulates a positional slip of 2.399 meters along the channel axis, while the scheme in this case only has a slip of 0.193 meters, which is an improvement of 12.44 times.

[0149] Sub-step 2: Solve and reconstruct the pose increment by direction.

[0150] For example, in this embodiment, the pose increment can be calculated as follows: First, the right-hand vector of the weighted normal equation of each source is projected onto the direction basis and weighted in each direction:

[0151]

[0152] Secondly, construct the direction-weighted information matrix and take its Rayleigh quotient in each direction:

[0153]

[0154] Finally, the six-dimensional pose increment is calculated separately for each direction and then linearly combined:

[0155]

[0156]

[0157] in, For the first The source right-hand vector is at the th Coordinate components in each direction; This is the information matrix after direction weighting; For its in the Rayleigh in each direction; The reference damping coefficient can be 0.01 in this embodiment; This is the prior pose increment synthesized in sub-step 1; This is the final six-dimensional pose increment.

[0158] Understandably, writing the damping term as the product of the square of the degradation degree and the largest eigenvalue, rather than a constant, carries profound numerical significance. In benign directions, the degradation degree approaches zero, the damping term tends to vanish, and the solution degenerates into a pure Gauss-Newton step, maintaining quadratic convergence. In directions approaching degradation, the degradation degree approaches 1, and the damping term tends to be the product of the baseline damping coefficient and the information content in the strongest direction, sufficient to suppress the increment in that direction to the same scale as the strongest direction, preventing numerical explosion. The square form keeps the damping moderate in the moderate degradation range, only increasing sharply when truly approaching an ill-conditioned state.

[0159] It should be noted that the direction-weighted information matrix is ​​generally not strictly diagonal under the original direction basis. This is because the principal axes of the source information matrices do not coincide with the principal axis of the laser information matrix, leaving off-diagonal coupling terms after weighting in each direction. The magnitude of these coupling terms is determined by the cross terms of the information quantities in each source direction, and in this scenario, they are usually one to two orders of magnitude lower than the diagonal terms. Furthermore, since the solution is obtained iteratively, the residual couplings will be automatically absorbed in the next iteration. If extremely high accuracy is required, a Jacobian rotation can be performed again after weighting to re-diagonalize the matrix. The cost is an additional sixth-order symmetric eigenvalue decomposition per frame, which takes less than ten microseconds on modern embedded processors and therefore does not pose a real-time obstacle.

[0160] Step 6: Construct and continuously update a three-layer spatiotemporal voxel map consisting of static, semi-static, and dynamic layers.

[0161] Among them, the three-layer spatiotemporal voxel map refers to the process of dividing the environmental space into voxels, maintaining two orthogonal dimensions—occupation confidence and change frequency—for each voxel, and classifying the voxel into a static, semi-static, or dynamic layer of environmental representation based on this.

[0162] Static layers refer to the voxels occupied by structures that exist for a long time and hardly change, such as load-bearing columns in a factory, the steel frame of a shelf, floors, ceiling trusses, and fire pipes. Semi-static layers refer to the voxels occupied by targets that appear and disappear repeatedly on a scale of hours to days, such as goods on shelves, temporarily stacked pallets, and turnover boxes. Dynamic layers refer to the voxels occupied by targets that move on a scale of seconds, such as personnel, forklifts, and other robots.

[0163] Occupancy logarithmic probability refers to the probability of a voxel being occupied, calculated as the natural logarithm. Its range is the entire real axis, with positive values ​​indicating a tendency towards occupation, negative values ​​indicating a tendency towards vacancy, and zero indicating uncertainty. Flip rate is the ratio of the number of times a voxel's occupancy state crosses the neutral line in its observation history to its cumulative observation duration. Regional variability is the weighted average of the flip rates of all voxels within a spatial neighborhood centered on a given voxel.

[0164] The reason for introducing the flip rate, a second dimension orthogonal to the occupancy log probability, is that traditional occupancy grid maps only answer whether something exists at a location. However, in a logistics warehouse, the steel frame of the shelves remains unchanged for years, the cardboard boxes on the shelves are replaced every few hours, and the forklifts passing by move only for a few seconds. All three occupy the same location in the occupancy grid. Using such a map for registration is equivalent to treating three objects with millions of times different lifespans the same, inevitably resulting in a map that quickly becomes outdated, residuals being contaminated by a large number of invalid points, an increase in the number of iterations, and pose drift. Figure 10 This embodiment shows a comparison of the matching interior point rates between a single-layer map and a three-subset environment model during the map aging process; from Figure 10 As can be seen, on the 7th day after map construction, the matching in-point rate of the single-layer map has dropped to 0.38, while the matching in-point rate of the first subset of the three-subset environment model remains at 0.918, with a difference of 0.538 between the two.

[0165] In this embodiment, in order to enable the map to automatically distinguish targets with different time scales and to automatically remove outdated observational evidence, the construction and updating of the three-layer spatiotemporal voxel map may specifically include the following sub-steps:

[0166] Sub-step 1: Map initialization.

[0167] In this embodiment, all occupied voxels collected during the mapping phase are first established as static layer candidates. Their occupancy log probability is initialized according to the number of observations during the mapping period, the flip count is set to zero, and the cumulative observation time is set to the mapping time. In the first few update cycles after the robot is put into operation, each voxel is gradually re-layered according to its actual observed flip situation.

[0168] It should be noted that the reason for adopting the initialization strategy of first making all candidates static and then gradually separating semi-static ones, instead of the other way around, is that mapping is usually completed all at once during the warehouse's idle period. At this time, no changes can be observed. If all are made semi-static by default, there will be no high-confidence static constraints available in the early stage of operation, and the robot's positioning quality will be significantly worse than in the stable period in the first few hours of production.

[0169] Sub-step 2: Ray projection update with time oblivion.

[0170] Ray projection refers to the method of discretizing each ranging ray of the lidar in voxel space, applying hit evidence to the voxel where the ray terminates, and applying penetration evidence to the voxels along the ray's path. The temporal forgetting factor refers to the exponential decay applied to the logarithmic probability of a voxel's existing occupancy based on the time interval since the last update before applying evidence for the current frame.

[0171] In this embodiment, to automatically remove cargo observed three days ago from the evidence set, thereby enabling the map to adaptively update according to changes in the field, the specific implementation of raycasting update with forgetting is as follows:

[0172] For each ranging ray, discretize it according to voxel resolution to obtain its endpoint voxel and the voxels that penetrate along the way.

[0173] For each voxel to be updated, first calculate the forgetting factor based on the time interval since the last update and the evidence half-life of the layer to which it belongs, and then use this factor to scale its existing occupancy log probability.

[0174] For endpoint voxels, the hit evidence is accumulated; for penetrating voxels, the penetration evidence is deducted. Both types of evidence are reduced by distance based on the range measurement value.

[0175] Update the voxel's most recent hit time and cumulative observation duration, and add a flip count when its log probability crosses the neutral line.

[0176] For example, in this embodiment, the voxel occupancy log probability can be updated using the following formula:

[0177]

[0178] in, voxels The logarithmic odds of occupancy; This represents the time interval since the last update for this voxel; To occupy the half-life of the evidence, in this embodiment, the value can be taken according to the different layers to which the voxel belongs: 72 hours for static layers and 4 hours for semi-static layers. and These are the sets of rays that terminate at this voxel and those that penetrate this voxel, respectively; and The evidence for a single hit and penetration can be taken as 0.85 and 0.4 respectively; The evidence reduction function is used to reflect the decrease in spatial resolution of distant rays, as the distance increases.

[0179] Understandably, the exponential forgetting factor works by pulling the logarithmic probability back to zero (i.e., a neutral state occupying half the probability), rather than towards idleness. This is crucial: the correct state description for a voxel that has not been observed for a long time should be unknown, not definitively empty. Otherwise, when the robot re-enters a long-unvisited area, it will fail to match due to the large inconsistency between the map and the actual location.

[0180] Sub-step 3: Determine the three-layer attribution based on the logarithmic probability of occupancy and the flip rate, and calculate the drasticness of regional changes.

[0181] For example, in this embodiment, the three-layer affiliation and the degree of regional change can be calculated using the following formula:

[0182]

[0183]

[0184]

[0185] in, This represents the number of times the voxel's occupied state has flipped across the neutral line during the observation history. This represents the cumulative duration of observation for that voxel; For flip rate; and To determine the upper and lower thresholds for the logarithmic probability, in this embodiment, they can be set to 2.0 and 1, respectively. ; The flip rate that marks the boundary between static and semi-static layers can be set to 0.05 times per hour. The set of hit voxels in the current frame; For voxels A spatial neighborhood centered at a point with a side length equal to several times the voxel resolution; The coordinates of the voxel center; It is a spatial kernel function that monotonically decreases with distance; This represents the degree of drastic change in the region.

[0186] Understandably, the dynamic layer's criterion requires both a historical occupancy logarithmic probability below a lower threshold and a hit in the current frame to be met simultaneously. This is because only in this way can moving objects be distinguished from newly placed goods: the former's voxels were previously empty (rays could pass freely), while the latter's voxels often had some prior occupancy evidence. If only a hit in the current frame and no record in the map were used as the criterion, newly placed pallets would be misjudged as dynamic obstacles and removed, thus losing the semi-static constraints that could have been utilized.

[0187] It should be noted that the reason for calculating the regional drasticness of change separately from the individual voxel flip rate is that the handling of goods often occurs on a unit basis, such as an entire storage location or a row of shelves, with a spatial scale much larger than that of a single voxel; moreover, the flip count of individual voxels is greatly affected by the randomness of observation sampling and is statistically unstable. Using the neighborhood weighted average as a measure of regional change can more accurately reflect how frequently items are changed in this area, and also ensures that the subsequent matching dependency reduction is spatially continuous and does not exhibit voxel-by-voxel jumps.

[0188] Step 7: Select the set of points to participate in the registration according to the adaptive hierarchical strategy of prioritizing static layers, and coordinate the sampling resolution, matching frequency and sliding window length according to the motion state and computing power budget.

[0189] The adaptive hierarchical strategy refers to a point set selection method that, by default, only uses points within the static layer voxels for registration, adding points from the semi-static layer voxels only when the static constraints are insufficient, and removing points from the dynamic layer voxels before residual calculation. The constraint sufficiency lower bound refers to the minimum eigenvalue threshold used to determine whether the current point set participating in registration is sufficient to constrain all six degrees of freedom.

[0190] The reason for adopting a static layer priority strategy is that the speed comes from the reduction of the number of points. The number of points in the static layer usually only accounts for 30% to 40% of the total number of voxels. Using only the static layer for matching means that the scale of the nearest neighbor search and the number of residual terms are greatly reduced. The source of non-drift is the quality of the points. All points in the static layer correspond to long-term existing geometric structures and have the highest consistency with the map. The residuals do not contain false point pairs that exist on the map but not on the ground. Figure 11 This diagram shows a comparison of the number of participating points and the registration time per frame between full-point participation registration and static layer-priority layer-by-layer selection in this embodiment; from Figure 11 As can be seen, the static layer-priority hierarchical point selection reduced the number of points participating in registration from 12,000 to 4,320, a decrease of 64.0%, and the registration time per frame was reduced from 6.66 milliseconds to 3.39 milliseconds, a decrease of 49.1%. This can simultaneously reduce the most expensive nearest neighbor retrieval scale and the number of iterations increased due to map mismatch.

[0191] In this embodiment, in order to obtain the aforementioned acceleration and anti-drift benefits while avoiding loss of observability due to insufficient points in sections with sparse static features (such as isolated islands surrounded by tall pallets on all sides, or passageways where the steel frame is completely obscured by goods), the selection and scheduling of the registration point set may specifically include the following sub-steps:

[0192] Sub-step 1: Layered point selection and constraint sufficiency triggering.

[0193] In this embodiment, the specific implementation method of layered point selection is as follows:

[0194] First, select only the points that fall into the static layer voxels, construct the information matrix in the manner of step 2, and take its minimum eigenvalue;

[0195] When the minimum eigenvalue is lower than the constraint sufficiency lower limit, additional points falling into the semi-static layer voxels are selected, and the residual weights of the additional points are double-reduced according to the survival probability of the voxel and the drastic change of the region.

[0196] Points falling into the dynamic layer voxels are removed before the nearest neighbor search and are not included in the residual calculation;

[0197] Points in the static layer are assigned a stricter nearest neighbor distance threshold, while points in the semi-static layer are assigned a wider nearest neighbor distance threshold.

[0198] For example, in this embodiment, the hierarchical residual weight, survival probability, and hierarchical nearest neighbor threshold can be calculated using the following formula:

[0199]

[0200]

[0201] in, The residual weighting coefficient for the point cloud points falling into this voxel; This is the baseline reduction factor for the semi-static layer, with a value range of [value range missing]. In this embodiment, 0.6 can be used; The survival probability of this voxel is obtained by the logarithmic probability of occupancy through the logistic inverse transformation, and its value always falls within the range of 1 / 2. Within the range; This represents the saturation value for the intensity of regional changes. This indicates taking the non-negative part; The smallest eigenvalue of the information matrix constructed using only static layer points; To constrain the lower limit of sufficiency; and The nearest neighbor distance thresholds for the static and semi-static layers are respectively, satisfying... In this embodiment, the lengths can be 0.15 meters and 0.4 meters respectively.

[0202] Understandably, the reduction factor, which is composed of the intensity of regional changes, is a quantitative expression of the automatic reduction of the matching dependence of a region when the characteristics of a region change drastically: when the intensity of change of a certain storage location reaches the saturation value, the factor returns to zero, which means that even if the semi-static points in the region are added, they will not contribute any constraint. The robot no longer depends on the map of this area, but instead relies on the static structure of other sections and other observation sources.

[0203] It's important to note that the removal of dynamic layer points occurs before, rather than after, residual calculation. This difference in order is significant: if residuals are calculated first and then removed, dynamic points still need to participate in the most expensive step of nearest neighbor search; by filtering by layer first and then calculating, the input size for nearest neighbor search is directly reduced. In sorting areas with high concentrations of personnel and forklifts, dynamic points can account for 20-30% of the points in a single frame, and the time saved by this order adjustment is considerable.

[0204] Sub-step 2: Coordinated scheduling of sampling resolution, matching frequency and sliding window length.

[0205] Cooperative scheduling refers to the process of solving the downsampled voxel side length, registration execution frequency and the number of backend sliding window keyframes in sequence according to the current geometric constraint sufficiency, motion state, pose uncertainty growth rate and computing power budget, based on the determined sequential dependencies.

[0206] The reason for collaborative scheduling is that the operating conditions of logistics robots are highly heterogeneous: when speeding along a straight line at two meters per second on an open main road, the surrounding geometry is simple and the constraints are sufficient, but the highest resolution downsampling and the highest frequency matching are still used, resulting in a large waste of computing power; when entering a dense shelving area or aisle intersection, the geometry is complex and the risk of collision increases sharply, but the accuracy cannot be temporarily improved because the parameters are fixed; and the total computing power of the edge computing platform is limited and needs to be shared with modules such as perception, planning and communication, so the overhead of the positioning module must be subject to hard constraints. Figure 12 This diagram shows a comparison of processor usage and topology node neighborhood positioning error between the fixed parameter scheme and the cooperative scheduling scheme in this embodiment; from Figure 12 As can be seen, the average processor utilization of the collaborative scheduling scheme is 54.34%, which is only 0.936 times that of the fixed parameter scheme (58.04%). Meanwhile, the positioning error in the neighborhood of the topology node is reduced from 0.0846 meters to 0.0489 meters, a reduction of 42.2%. This scheme can concentrate computing resources on the section with the highest collision risk without increasing the total computing power utilization.

[0207] In this embodiment, in order to achieve the optimal positioning accuracy under the given processor time budget, the specific implementation of cooperative scheduling is as follows:

[0208] First, the upper bound of the voxel side length that just satisfies the observability is obtained by inverse solution of constraint sufficiency. Then, the expected kinematic side length is calculated by linear velocity and distance to the nearest map topology node. The smaller of the two is taken and the upper and lower bounds are used for truncation.

[0209] Then, by recursively deriving the trace growth rate of the pose covariance and the upper limit of the allowable accumulated position error between the two registrations, the minimum registration frequency required can be solved in reverse.

[0210] Finally, the portion already consumed by the front end is deducted from the total computing power budget, and the remaining budget is used to solve the maximum number of keyframes that can be tolerated using the backend complexity model.

[0211] For example, in this embodiment, the side length of the downsampling voxel can be calculated using the following formula:

[0212]

[0213]

[0214]

[0215] Furthermore, the registration execution frequency can be calculated using the following formula:

[0216]

[0217] Finally, the number of keyframes for the sliding window can be calculated using the following formula:

[0218]

[0219] in, The reference voxel side length can be taken as 0.2 meters in this embodiment; The minimum eigenvalue of the static layer point set information matrix under the reference side length; To constrain the lower limit of sufficiency, it is the same constant as sub-step 1; the one-third exponent comes from the inverse cubic relationship between the voxel side length and the number of retained points; This refers to the robot's linear velocity. As a reference speed, 1 meter per second can be taken; The speed relaxation factor can be set to 0.5; This represents the distance from the current location to the nearest map topology node (intersection of passageways, entrance to dense shelving area); The radius of influence of the node can be taken as 3 meters. The risk tightening coefficient can be set to 0.4. and For engineering purposes, the upper and lower limits of the side length can be taken as 0.1 meters and 0.5 meters respectively; The inertial recursive pose covariance; , , These are its continuous-time error state transition matrix, noise input matrix, and noise power spectral density matrix, respectively. The upper limit of the allowable accumulated positional error between two registrations can be taken as 0.03 meters; The lower limit of the frequency can be set to 5 Hz; The processor time budget allocated to the positioning thread; This represents the current percentage of available computing power. , , These are the front-end fixed overhead, single-time registration overhead, and back-end unit window overhead coefficients, respectively. The complexity exponent for solving factor graphs with respect to window length is approximately 1.5 for sparse pose graphs. and This represents the upper and lower limits of the number of keyframes.

[0220] Understandably, the smaller operation is the most crucial structure of this scheduling law. It ensures that the sufficiency of constraints always has a veto power over the speed of the robot: even at maximum speed, as long as the geometric constraints of the current static layer are already strained, the voxel side lengths will not be relaxed to the point of compromising observability. If a multiplicative form is used instead, the velocity term can push the side lengths beyond the information-preserving upper bound, rendering the design intent of preserving information ineffective at high speeds.

[0221] It should be noted that the trace of pose uncertainty grows at a certain rate. If it is allowed to grow to the square of the allowable error within the interval between two registrations, then the quotient of this rate and the frequency equals the square of the allowable error. Taking the square root yields the above formula. When traveling at a constant speed in a straight line, the error from inertial recursion grows more slowly, and the required frequency automatically decreases. When turning frequently, the error term caused by angular velocity increases significantly, and the required frequency automatically increases. This replaces the empirical rule of reducing frequency for high-speed straight travel and increasing frequency for complex maneuvers with an analytical law directly derived from the noise model. Furthermore, the three formulas constitute a strict unidirectional dependency chain. The entire chain can be solved in a fixed number of arithmetic operations within each scheduling cycle, without any iteration or search. This property is essential for real-time systems that must complete scheduling decisions in milliseconds; otherwise, the computing power saved by scheduling will be consumed by scheduling itself.

[0222] Step 8: When the robot pose is lost, perform rapid relocalization using a coarse-to-fine approach based on the heterogeneous spatial fingerprint.

[0223] Among them, pose loss refers to the state in which the robot no longer possesses a reliable prior pose due to unexpected power outages and restarts, manual handling, or prolonged occlusion causing tracking failure; this is often referred to as a kidnapping state. Heterogeneous spatial fingerprint refers to a fixed-length binary code formed by quantizing and splicing together the topological invariants of the constant top structure extracted from upward-view observations and the magnetic field distortion symbol sequence collected along the travel distance. Topological region refers to a map sub-interval corresponding one-to-one with a reference fingerprint, obtained by segmenting the edges of the map topology along equal distances during the mapping phase.

[0224] The reason for not using full-map scanning matching is that all thirty channels in the logistics warehouse look exactly the same. Any point cloud segment can be placed on any channel and matched well. As a result, the similarity function of full-map scanning matching exhibits a multi-peak distribution, and the robot is very likely to anchor to the wrong channel. Moreover, as the map size increases, the search time increases linearly, and the wake-up process can easily take several seconds to tens of seconds. Figure 13 This diagram illustrates a comparison of the kidnapping recovery time as a function of the environmental model size in this embodiment; from Figure 13 As can be seen, when the scale of the environmental model increases to 200,000 square meters, the time for kidnapping recovery by full-map scanning matching reaches 8.4 seconds and increases linearly with the scale, while the maximum time for spatial identifier retrieval plus local registration in the full scale range is only 0.368 seconds, with an improvement of 22.8 times at the largest scale.

[0225] The reason for using an upward perspective is that the geometry at ground level is both highly repetitive and highly dynamic within the warehouse, while the ceiling is the opposite. The routing of fire ducts, the array of lighting fixtures, and the intersections of trusses remain unchanged for years after the factory was built, and there are natural differences in the routing of pipelines and the density of lighting arrays in different sections. Pointing the camera upward is equivalent to switching from a highly repetitive and dynamic observation surface to a relatively unique and constant one.

[0226] The reason for superimposing the magnetic field distortion sequence is that the metal shelves will cause repeatable geomagnetic distortion around them, and this physical quantity is completely independent of geometry. Therefore, it is heterogeneous and complementary to the top topological features: even if the ceilings of the two aisles happen to be similar, the difference in the distribution of metal goods on their shelves will make the magnetic field sequence different.

[0227] In this embodiment, in order to ensure that relocation is not affected by geometric repetition and that the time consumption is essentially decoupled from the map size, fast relocation may specifically include the following sub-steps:

[0228] Sub-step 1: Extract scale- and orientation-independent topological invariants of the top constant structure.

[0229] Topological invariants refer to a set of scalar features that remain unchanged under rotation and uniform scaling transformations. Invariants must be used instead of absolute position and length because the robot does not know its orientation when it restarts, and the descriptor must be insensitive to rotation; the distance between the robot and the ceiling may also change due to the cargo height, so the descriptor must also be insensitive to scale.

[0230] For example, in this embodiment, the top topological invariant can be calculated using the following formula:

[0231]

[0232]

[0233] in, and The first image is the one viewed from the top. The direction angle and length of a straight line segment; For angle kernel function; and These are the principal and secondary principal directions of the length-weighted direction histogram. For model The minimum included angle after folding, i.e., the difference between the two direction angles and Subtract the smaller of the differences; The ratio of the first three values ​​of the distance between adjacent intersections, arranged in descending order, is used to eliminate the scale. The number of intersections within the field of view; Let be the field of view area.

[0234] It should be noted that the orientation angle must be modulo. The folding of the line segment. The direction of the line segment is a vectorless vector, and its natural range is from zero to... If not folded, the same geometric structure will give two different invariants depending on the order of the line segment endpoints, and the fingerprint will not be reproducible.

[0235] Sub-step 2: Perform equal-mileage resampling and ternary symbolization on the magnetic field distortion sequence.

[0236] For example, in this embodiment, the magnetic field symbol sequence can be calculated using the following formula:

[0237]

[0238]

[0239] in, The sequence is obtained by resampling at equal mileage steps to represent the distance traveled. In this embodiment, the step size can be 0.1 meters. A sliding window defined by mileage length; The magnetic field strength; and These are the median and absolute deviation of the median within the window, respectively. and The upper and lower quantile thresholds; It is a three-valued symbol sequence.

[0240] It is understandable that the median and median absolute deviation, rather than the mean and standard deviation, are used as the normalization benchmark because magnetic field sequences often contain isolated spikes caused by instantaneous strong magnetic sources such as nearby forklift motors; median-type statistics are not sensitive to these outliers and can ensure that the same passage gives a consistent sign sequence whether or not a forklift passes by.

[0241] It should be noted that the independent variable for resampling must be the distance traveled, not the time. If sampling is performed at a fixed time step, the same passage will produce sequences of different lengths and misaligned inflection points at different vehicle speeds, making fingerprint reproducibility impossible. Reproducibility is the very prerequisite for fingerprint retrieval to be valid.

[0242] Sub-step 3: Concatenate fingerprints, perform Hamming search, and conduct a second reprojection consistency check.

[0243] For example, in this embodiment, the fingerprint splicing and retrieval can be calculated using the following formula:

[0244]

[0245]

[0246] Furthermore, the reprojection consistency rate of the candidates can be calculated using the following formula:

[0247]

[0248] in, for In this embodiment, the bit uniform quantization operator can take 8 bits for each dimension of the top invariant and 2 bits for each sampling point of the magnetic field symbol; This is for stitching by position; The fingerprint mileage window is long enough to reach 12 meters. It is a set of indexes for all topological regions in the fingerprint database; For the first Reference fingerprints for each region; For bitwise XOR; That is, the Hamming distance; The one with the smallest Hamming distance A set of candidate regions 5 is acceptable; The pose obtained by registration in a local sub-map of the candidate region; The currently observed number Top line characteristics; For the reprojection operator based on this pose; This is the set of reference line features in the sub-map; Determine the distance to the interior point; For reprojection consistency rate; To accept the threshold, a value of 0.6 can be set.

[0249] It's important to note that the fingerprint database is built during the mapping phase: the map topology is segmented along each edge using equal mileage steps, a reference fingerprint is calculated for each segment, and its corresponding local sub-map index is attached, forming a fingerprint-sub-map mapping table. Therefore, runtime relocalization requires only two steps: first, a Hamming search is used to retrieve several candidate segments (this step only involves XORing and bit counting of a fixed-length bit string, with overhead almost independent of map size); then, fine registration is performed only on these candidate sub-maps. Compared to full-map scanning matching, the map size involved in geometric optimization is compressed from the entire map to a few sub-maps, which is precisely the order-of-magnitude difference brought about by coarse localization followed by fine localization.

[0250] It's important to note that the reprojection consistency check is a crucial step in preventing misanchoring and cannot be omitted. A small Hamming distance only indicates descriptor similarity, not geometric consistency. In a warehouse with highly repetitive geometry, two different channels might give similar Hamming distances. If the smallest Hamming distance is directly chosen as the result, the probability of misanchoring cannot be ignored. Adding the consistency check upgrades the decision criterion from descriptor similarity to both descriptor similarity and geometric consistency. Furthermore, when all candidates fail the check, the algorithm would rather report a relocation failure and retry with a larger number of candidates than output a potentially incorrect pose. For logistics robots, the consequences of a single incorrect anchoring leading to a path planning error are far more serious than spending a few hundred milliseconds. Figure 14 This embodiment shows a comparison of the correct anchoring rates of decisions based solely on inter-symbol distance and those based on superposition consistency testing; from Figure 14 As can be seen, when there are 5 candidate topology intervals, the correct anchoring rate based solely on the code distance is 0.493. After adding the consistency test, it rises to 0.999, an improvement of 0.506, and the gap between the two widens further as the number of candidates increases.

[0251] Sub-step 4: System state recovery after successful relocation.

[0252] In this embodiment, after successful relocation, the system restores to normal tracking status as follows: clear all historical keyframes and factors in the back-end sliding window; construct a new prior factor using the relocation pose and its covariance as the starting point of the window; reset and recalculate the weighting status and information history of all base stations in step 4; and clear the information source identifier set in step 9.

[0253] It should be noted that if the sliding window is not cleared and the repositioned pose is directly added as a new observation, the historical factors remaining in the window and established based on the incorrect pose will conflict with the new prior factors. The optimization result will be a compromise between the old and new poses, which will put the robot in an incorrect state that is neither the old pose nor the new pose.

[0254] Step 9: When the self-assessment location reliability diverges, trigger distributed collaborative error correction through multi-machine mutual observation.

[0255] Covariance divergence refers to the rapid increase of the trace of the local robot's pose covariance matrix over a set time period, or the significant increase of its component in a certain degradation direction. Relative pose refers to the six-degree-of-freedom rigid body transformation between the observer robot's body coordinate system and the requesting robot's body coordinate system. The information source identifier set refers to the set of identifiers maintained for each robot, recording all external information providers whose identifiers have been relied upon for its current pose estimation.

[0256] This mechanism is necessary because all the aforementioned mechanisms rely on the premise that the robot itself has at least one reliable observation path. However, an extreme scenario exists in a warehouse that breaks this premise: the robot is completely surrounded by tall pallets on all four sides, its laser can only see the surface of nearby cardboard boxes (geometrically degraded and these boxes are not part of the static layer), its upward view is blocked by the top of the pallets, and its radio frequency signals are completely shielded by the surrounding metal. In this situation, all onboard observations degrade simultaneously, and the robot has no external constraints to correct accumulated errors. If it is not desirable to lay additional fixed infrastructure in the warehouse (which is both expensive and difficult to maintain), the only way out is to seek help from a companion robot.

[0257] In this embodiment, in order to enable robots to be quickly recalled from blind spots even without any external fixed infrastructure, and to prevent the swarm from consistently deviating from the true value due to duplicate information input, the collaborative correction can specifically include the following sub-steps:

[0258] Sub-step 1: Trigger a help broadcast using the covariance divergence criterion.

[0259] For example, in this embodiment, the divergence criterion can be calculated using the following formula:

[0260]

[0261]

[0262] in, This is the local pose covariance matrix; For the evaluation time window; and These are the degradation degree and orientation basis obtained in step 2, respectively; Weights for the direction term; The threshold is used for divergence. The help request message carries a description of the machine's geometric profile (e.g., vehicle dimensions, coded pattern of the top active identifier) ​​along with the current pose estimate and its covariance.

[0263] Understandably, the reason for adding a direction term weighted by degradation degree in addition to the growth rate of the covariance trace in the criterion is that the typical characteristic of a trapped working condition is that one or two directions completely lose their constraints, rather than all directions deteriorating slowly together; if only the total trace is considered, the robot may trigger a distress call only after it has deviated several meters in a certain direction. This direction term shares the same set of direction bases with the degradation determination in step 2, which reflects the inherent consistency of this method.

[0264] Sub-step 2: Solve for relative pose and perform time delay alignment and covariance propagation.

[0265] In this embodiment, when the robot receiving the request for help meets the confidence condition, it calculates the relative pose and its covariance between the two by matching the geometric contour description of the requester in its laser point cloud or image. Then, it synthesizes the absolute pose prior of the requester in the following manner:

[0266] By recursively extrapolating the absolute pose of the sending time to the receiving time of the requesting party using the observer's own inertia;

[0267] The extrapolated absolute pose of the observer is combined with the relative pose to obtain the prior absolute pose of the requesting party.

[0268] The pose covariance of the observer is propagated using the Lie group adjoint transformation, and then superimposed with the relative pose observation covariance and the additional covariance introduced by time delay extrapolation.

[0269] For example, in this embodiment, delay alignment and covariance propagation can be calculated using the following formula:

[0270]

[0271]

[0272] in, The absolute pose of the observer at the time of transmission is represented by a homogeneous transformation; The pose increment between transmission and reception is obtained by the observer recursively from its own inertia. For the antisymmetric mapping of Lie algebras; The observation azimuth pose after time delay compensation; The relative pose calculated by the observer; The absolute pose prior of the party requesting assistance; As the adjoint operator of the Lie group, this expression adopts the right perturbation convention; and These are the absolute pose covariance of the observer and the relative pose observation covariance, respectively. The additional covariance introduced by time delay extrapolation.

[0273] It should be noted that the latency compensation step, though seemingly minor, is actually indispensable. From the time the observer collects data, calculates the relative pose, encodes and sends it, to the time the requester receives and decodes it, there is a delay of tens of milliseconds; during this time, the observer itself is also in motion. If the pose at the moment the observer sends the data is used directly, the prior edge will have a system bias proportional to the observer's vehicle speed. With a typical vehicle speed of 1.5 meters per second and an end-to-end latency of 40 milliseconds, this bias can reach 6 centimeters, which is on the same order of magnitude as the positioning accuracy pursued by this method, and is sufficient to offset the benefits brought by the cooperative correction itself.

[0274] Sub-step 3: Information source deduplication and prior edge injection.

[0275] For example, in this embodiment, the injection rule for prior edges can be expressed as:

[0276]

[0277]

[0278] in, For the logarithmic mapping of the Lie group and take its vector form; and These are sets of information source identifiers for the requesting party and the observing party, respectively. The prior edge cost term for the local factor graph of the requester is in Mahalanobis norm form, with the inverse of the prior covariance as the weight matrix.

[0279] It's important to note that the intersection and deduplication of the information source sets is the most crucial design element in this step. If the pose of vehicle A was already obtained from the correction by vehicle B, using vehicle A to correct vehicle B again is equivalent to counting the same information twice. The consequence isn't divergence, but rather the more dangerous overconfidence. The covariance is artificially compressed to a level far smaller than the true error, causing the entire fleet to consistently deviate from the true value while reporting high confidence levels, undetectable from outside the system through any self-evaluation indicators. After each prior knowledge is received, the information source set is incorporated into the other party's source set and identifier according to the above formula. It is then cleared after the requesting party regains confidence through independent observation (e.g., seeing the load-bearing column of the static layer again, or successfully completing a fingerprint-based relocation), thus forming a complete contamination-cleaning cycle. Figure 15 This diagram illustrates the impact of source deduplication on the consistency of uncertainty measurement in multi-machine collaborative correction in this embodiment; from Figure 15 As can be seen, after 10 rounds of swarm coordination, the ratio of uncertainty measure to the square of true error was underestimated to 0.278 without source deduplication, while after source deduplication, the ratio remained stable at 0.947, thus avoiding the necessary condition of the swarm consistently deviating from the true value while simultaneously reporting high confidence.

[0280] After the above steps, the robot obtains a complete set of outputs in each localization cycle: First, the current keyframe pose obtained by the back-end sliding window optimization, i.e., the six-degree-of-freedom rigid body transformation of the body coordinate system relative to the map coordinate system; second, the covariance matrix of the pose and its components under the orientation basis, used to indicate the confidence level of each direction to downstream modules; third, the current set of degenerate directions and the sequence of gating coefficients, which can be used by the path planning module to actively reduce speed or adjust the path in degenerate sections; fourth, the three-layer map incremental update results for this cycle, including voxels of the new layer, voxels whose occupancy log probability changes exceed the threshold, and voxels whose flip counts change.

[0281] When releasing data to the public, the latest optimized pose is used as the baseline. The pose and covariance are recursively extrapolated from the inertial measurement data to the current moment and released at a fixed frequency (100 Hz in this embodiment). This ensures smooth and continuous pose output even when the registration frequency is reduced by the scheduler. Map increments are packaged and uploaded to the map server at set intervals. They are then merged with increments from other robots on the server before being distributed, allowing all robots in the same warehouse to share the same continuously evolving three-layer spatiotemporal map.

[0282] Example 2

[0283] This embodiment discloses an adaptive rapid localization system for a robot, which is used to implement the adaptive rapid localization method for the robot in Embodiment 1.

[0284] In this embodiment, the system includes, in terms of hardware, a sensor array mounted on the robot body, an on-board edge computing unit, a vehicle-to-vehicle communication module, and a map server connected to it via a wireless network.

[0285] The sensor group includes a lidar unit, an inertial measurement unit, a wheel encoder, an RF ranging terminal, and a camera mounted towards the ceiling. The sampling clocks of each sensor are aligned to the same time base via hardware triggering or a network time protocol. The onboard edge computing unit is an embedded computing platform with a multi-core processor, responsible for all front-end and back-end computations of the positioning algorithm. The processor time budget allocated to the positioning thread is given by the system configuration file and serves as the input for collaborative scheduling in step 7 of Example 1. The vehicle-to-vehicle communication module supports short-range direct communication between robots and is used to carry the distress message, relative pose observation, and information source identifier set in step 9 of Example 1. The map server is responsible for the centralized storage, incremental merging, and version distribution of the three-layer spatiotemporal voxel map and fingerprint database.

[0286] In this embodiment, the modules can be integrated into the same electronic device, which may be an in-vehicle industrial control computer, an embedded computing board, or a domain controller; alternatively, they can be deployed according to the computing load distribution, for example, deploying the incremental merging part of the spatiotemporal mapping module on a server or server cluster, and deploying the remaining modules on an in-vehicle edge computing unit. The functions of the above modules can also be implemented by a computer program stored on a computer-readable storage medium. When the computer program is executed by the processor, it implements the adaptive rapid localization method for the robot described in Embodiment 1.

[0287] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An adaptive rapid localization method for a robot, characterized in that, The method is executed by the robot's processing equipment, and the adaptive rapid localization method includes: Acquire first observation data and second observation data. The first observation data is structural observation data of the environment in which the robot is located. The acquisition principle of the second observation data is different from that of the first observation data. Based on the sensitivity of the robot pose to the registration deviation between the first observation data and the pre-stored environment model, a target constraint metric is determined, and multiple mutually orthogonal pose directions and constraint strength indices for each pose direction are determined by the target constraint metric. The constraint strength indices indicate the constraint capability of the first observation data on the corresponding pose direction. For each of the pose directions, a shrinkage coefficient is determined according to the constraint strength index of the pose direction. The shrinkage coefficient is used to shrink the confidence weight of the first observation data in the pose direction and increase the confidence weight of the second observation data in the pose direction accordingly. The shrinkage coefficient decreases as the constraint strength index decreases. The first observation data and the second observation data are fused according to the confidence weights in each of the pose directions to obtain and output the pose of the robot.

2. The adaptive rapid localization method for a robot according to claim 1, characterized in that, The determination of the target constraint metric includes: For each observation element in the first observation data, determine the registration deviation between the observation element and the corresponding part in the environment model, and determine the sensitivity vector of the registration deviation to each component of the robot pose. The sensitivity vector is weighted by the inverse of the observation noise of the observed element, and the weighted results of all observed elements are summed to obtain the target constraint metric. The observation noise increases with the increase of the distance between the observation element and the robot, and also increases with the increase of the angle between the orientation of the environmental structure at the observation element and the observation line of sight of the observation element. The constraint strength index is the ratio of the metric value of the target constraint in the pose direction to the largest metric value in all pose directions.

3. The adaptive rapid localization method for a robot according to claim 1, characterized in that, Before determining multiple mutually orthogonal pose directions by the target constraint metric, the method further includes: The scale quantity is determined based on the distance of each observation element relative to the robot in the first observation data; The angular component of the target constraint metric related to the pose is converted into a component with the same dimension as the length component of the pose using the aforementioned scale quantity, thus obtaining a normalized target constraint metric. The plurality of mutually orthogonal pose directions are determined by the normalized target constraint metric.

4. The adaptive rapid localization method for a robot according to claim 1, characterized in that, The adaptive fast localization method further includes: The pose direction where the constraint strength index is lower than the first threshold is determined as the constraint missing direction. In the fusion, a damping amount is applied to each of the pose directions, and the damping amount increases as the constraint strength index of that pose direction decreases; Based on the degree of concentration of the orientation of the environmental structure at each observation element in the first observation data, the structural orientation distribution index is determined; The step of determining the pose direction where the constraint strength index is lower than the first threshold as the constraint missing direction includes: determining the pose direction as the constraint missing direction only when the constraint strength index of the pose direction is lower than the first threshold and the structure orientation distribution index is lower than the second threshold.

5. The adaptive rapid localization method for a robot according to claim 4, characterized in that, The first threshold includes an entry threshold and a release threshold that are not equal to each other, wherein the release threshold is greater than the entry threshold; The step of determining the pose direction where the constraint strength index is lower than the first threshold as the constraint missing direction includes: If the constraint strength index in the pose direction is lower than the entry threshold, the pose direction is determined to be a constraint-deficient direction. When the constraint strength index in the pose direction is higher than the release threshold, the determination of the missing constraint direction in the pose direction is released. When the constraint strength index of the pose direction is between the entry threshold and the release threshold, the determination result of the pose direction at the previous moment is maintained. The fusion includes: For pose directions that are not the direction of missing constraints, the pose components in that pose direction are calculated from the first observation data and the second observation data according to the confidence weight in that pose direction. For the direction where the constraint is missing, the component of the prior pose increment in the direction where the constraint is missing is taken as the pose component in the direction where the constraint is missing. The prior pose increment is calculated from observation data that does not depend on the environment model. The pose components in each of the aforementioned pose directions are synthesized according to the plurality of mutually orthogonal pose directions to obtain the pose of the robot.

6. The adaptive rapid localization method for a robot according to claim 1, characterized in that, The contraction coefficient and the constraint strength index are a monotonic mapping relationship without trainable parameters. The monotonic mapping relationship makes the contraction coefficient approach zero when the constraint strength index approaches zero, the contraction coefficient approach one when the constraint strength index is greater than a half-point threshold of a set multiple, and the contraction coefficient equal to one-half when the constraint strength index is equal to the half-point threshold. Before shrinking the confidence weight of the first observation data in the pose direction with the shrinkage coefficient, the method further includes: replacing the shrinkage coefficient used in the current positioning cycle with a weighted sum of the shrinkage coefficient determined in the current positioning cycle and the shrinkage coefficient used in the previous positioning cycle. The confidence weight of the second observation data in that pose direction is increased accordingly, including: In each of the pose directions, the sum of the confidence weights of the first observation data and the second observation data is kept constant, so that the confidence weight share of the first observation data is reduced and transferred to the second observation data according to the ratio of the constraint capability of the second observation data in that pose direction.

7. The adaptive rapid localization method for a robot according to claim 1, characterized in that, The second observation data includes: ranging observation data, which consists of the distance between a reference point with a known location and the robot; The adaptive fast localization method further includes: The predicted distance between the robot and the reference point is calculated based on the prior pose increment, and the difference between the ranging observation data and the predicted distance is determined as the deviation. The prior pose increment is calculated from the observation data that does not depend on the environment model. When the deviation is positive and greater than the first threshold, the confidence weight of the ranging observation data is reset to zero; when the deviation is negative and its absolute value is greater than the second threshold, the confidence weight of the ranging observation data is reset to zero. Where the first threshold is smaller than the second threshold; When the deviation is positive, greater than the set lower transition boundary, and not greater than the first threshold, the confidence weight of the ranging observation data is determined monotonically decreasing according to the absolute value of the deviation.

8. The adaptive rapid localization method for a robot according to claim 1, characterized in that, The environmental model consists of multiple spatial units, each of which corresponds to a survival index and a change frequency index. The survival index indicates the confidence level of the spatial unit being occupied, and the change frequency index indicates the frequency of changes in the occupancy status of the spatial unit. The environment model includes a first subset, a second subset, and a third subset. The first subset is a set of spatial units whose survival index is higher than the upper survival threshold and whose change frequency index is not higher than the change threshold. The second subset is a set of spatial units whose survival index is higher than the upper survival threshold and whose change frequency index is higher than the change threshold. The third subset is a set of spatial units whose survival index is not higher than the lower survival threshold and whose occupied space is observed in the current positioning period. The first observation data used in determining the target constraint metric is selected in the following manner: First, select only the observation elements corresponding to the first subset, and determine the smallest metric value in the target constraint metric composed of this part of the observation elements; When the minimum metric value is lower than the sufficiency lower limit, additional observation elements corresponding to the second subset are selected, and the confidence weight of the observation element is reduced according to the survival probability of the spatial unit corresponding to the observation element. The survival probability is obtained by monotonic mapping of the survival index of the spatial unit and takes a value between zero and one. The observation elements corresponding to the third subset are discarded before the registration bias is determined; The reduction is also based on the regional variation index, which is the weighted average of the variation frequency indices of each spatial unit in the neighborhood of the spatial unit. The larger the regional variation index, the stronger the reduction. Furthermore, the corresponding part retrieval threshold used by the observation element corresponding to the first subset when determining the registration deviation is less than the corresponding part retrieval threshold used by the observation element corresponding to the second subset.

9. The adaptive rapid localization method for a robot according to claim 1, characterized in that, To address the situation where the robot loses its pose, the adaptive fast localization method further includes: Structural invariants are extracted from observation data acquired by the robot from above, and these structural invariants remain unchanged under rotation and scaling transformations. Field strength sequences are collected at equal mileage intervals along the robot's travel distance. The field strength sequences are normalized within a sliding mileage window and then converted into field strength symbol sequences according to graded thresholds. The structural invariant and the field strength symbol sequence are quantized and concatenated into a fixed-length spatial identifier code. The spatial identifier code is then searched with multiple pre-established reference spatial identifier codes according to the inter-code distance to obtain several candidate topological intervals. Only the local environment model corresponding to the candidate topology interval is retrieved for registration, and the registration result is used as the recovered pose after the robot pose is lost. Before using the registration result as the recovered pose after the robot pose loss, the method further includes: Based on the registration result of the candidate topology interval, the observation features obtained by the robot facing upwards are projected onto the local environment model corresponding to the candidate topology interval. The proportion of observation features whose distance from the reference feature in the local environment model after projection is less than the inlier distance is calculated to obtain the consistency index. The registration result of the candidate topology interval is accepted only when the consistency index is higher than the acceptance threshold; when the consistency index of all candidate topology intervals is not higher than the acceptance threshold, the number of candidate topology intervals is increased and the search is repeated.

10. An adaptive rapid positioning system for a robot, characterized in that, The adaptive rapid positioning system includes: processor; The memory stores a computer program that, when executed by a processor, implements the adaptive rapid positioning method for the robot as described in any one of claims 1 to 9.

Images