Mobile node spatial positioning orientation and autonomous navigation method facing warehouse direction
By constructing a topological baseline storage and a factor graph optimization model, and combining visual sensors and physical attribute labels, the problem of unstable positioning calculation for mobile robots in dynamic warehouse environments was solved, and robust autonomous navigation was achieved.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- SHANXI WANCHUANG TECH CO LTD
- Filing Date
- 2026-05-13
- Publication Date
- 2026-06-09
AI Technical Summary
In highly dynamic warehouse environments, existing mobile robot autonomous navigation relies on visual feature matching, which leads to unstable localization solutions and cumulative orientation errors. In particular, when goods are stacked and changed, the lack of deterministic constraints causes the pose observation equation to diverge.
By constructing a topological benchmark storage, combining the 3D coordinates of feature points and physical attribute labels obtained by visual sensors, a deep coupling mechanism between semantic labels and topological constraints is established. By utilizing multidimensional spatial retrieval indexes and factor graph optimization models, the covariance weights and physical boundary constraint parameters are adjusted to achieve logical extraction of effective spatial features and stability compensation for pose calculation.
In a dynamic warehousing environment, the system achieves global robustness and adaptability of positioning results, suppresses the impact of cargo interference on the navigation system, and maintains the continuous positioning and orientation capabilities of mobile nodes.
Smart Images
Figure CN122170902A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of positioning and navigation technology, and particularly relates to a method for spatial positioning, orientation and autonomous navigation of mobile nodes oriented towards warehousing. Background Technology
[0002] Currently, autonomous navigation of mobile robots relies on the extraction of environmental features by a visual perception system. By calculating the spatial relative position between the carrier and environmental feature points, the six-degree-of-freedom pose parameters of the mobile node in the three-dimensional coordinate system are determined. Warehouse operation environments are highly dynamic. Frequent handling of pallets, shelves, and goods causes disordered displacement of physical features within the field of view. Existing navigation methods usually treat the environment as a random probability field, relying on gray-level gradients or geometric consistency matching of feature points. When a large number of visual features deviate from their physical positions due to the operation process, matching methods based solely on statistical laws cannot identify the attribute of feature points, resulting in continuous residuals in the pose observation equation. This phenomenon reflects the essential contradiction between the requirement for feature stability and the variability of the physical environment, leading to jumps or cumulative orientation errors in pose calculation.
[0003] To address the aforementioned issues, improvements such as increasing the number of sensors or introducing deep learning models to implement semantic masking are constrained by system power consumption and deployment costs in practical engineering applications. Simply relying on visual data cleaning cannot verify, from a fundamental logical perspective, whether the displacement of feature points conforms to the architectural structural design specifications of the warehouse space. This causes the system to diverge in the solution process when facing large-scale cargo stacking changes due to a lack of deterministic constraints. For example, Chinese invention patent application CN119984252A discloses a visual inertial odometry positioning method based on NCC dynamic covariance adjustment. It uses normalized cross-correlation (NCC) as a quantification indicator of feature point matching quality and dynamically adjusts the observation noise covariance. This algorithm logic, which relies on the surface statistical regularities of visual images, ignores the semantic attributes of the physical entities to which the feature points belong and the topological constraints within the specific engineering space. In warehouse operations, pallets or goods in a translational state possess extremely high visual clarity and matching consistency. However, judging solely based on image correlation, such as NCC scores, the system is prone to misclassifying dynamic targets that violate the global static preset premise as high-quality positioning benchmarks, leading to fundamental deviations or divergences in pose calculation.
[0004] Therefore, how to establish a structured retrieval mapping channel between real-time visual features and a topological reference storage based on computer-aided design reconstruction, and realize the logical extraction of effective spatial features and stability compensation for pose calculation, has become the technical problem to be solved by this invention. Summary of the Invention
[0005] This invention provides a method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, comprising the following steps:
[0006] Step S1: The original feature stream of the warehouse scene, the physical attribute labels corresponding to the original feature stream, and the displacement increment data of the underlying drive motor are synchronously acquired through the multi-angle vision sensor array carried by the mobile node. The three-dimensional coordinate set of each feature point in the visual image frame is extracted, and the pose transformation matrix of the mobile node relative to the local coordinate system is determined according to the displacement increment data of adjacent sampling periods.
[0007] Step S2: Analyze the initial computer-aided design geometric topology model of the warehousing environment, extract the spatial distribution boundaries of shelves, aisles, pallets and goods, construct a topological benchmark storage body containing spatial constraints, define fixed shelves and aisles as static feature sets with fixed three-dimensional coordinate extreme values, define movable pallets and goods as dynamic feature sets associated with preset allowable motion displacement ranges, and establish a dictionary of association rules representing physical motion boundaries between the static feature set and the dynamic feature set.
[0008] Step S3: Construct a multi-dimensional spatial retrieval index using the three-dimensional coordinate set of feature points, and combine it with physical attribute labels to retrieve physical boundary constraint parameters that match the current coordinate region of the feature points in the topological reference storage via the data bus.
[0009] Step S4: Compare the measured displacement vectors and allowable displacement intervals of feature points between adjacent image frames, adjust the covariance weights of the feature observation components in the state observation equation according to the physical boundary constraint parameters, and combine the pose transformation matrix to complete the calculation of the six-degree-of-freedom pose parameters of the moving node.
[0010] Preferably, step S2 specifically includes: reading the geometric topology model of the storage environment and extracting boundary geometric features; constructing a static feature set representing the spatial positional relationship between shelves and aisles based on the boundary geometric features, and setting the motion envelope range of pallets and goods to construct a dynamic feature set; establishing an association rule dictionary representing the physical motion boundary between the static feature set and the dynamic feature set, and completing the reconstruction of the topology benchmark storage body.
[0011] Preferably, step S3 specifically includes: constructing the underlying architecture of a multidimensional spatial retrieval index using the three-dimensional coordinate set of feature points, and dividing feature points with the same attributes into independent feature clusters based on physical attribute labels; extracting the geometric center coordinates and three-dimensional envelope boundaries of the independent feature clusters, and dimensionally fusing the geometric center coordinates, three-dimensional envelope boundaries, and physical attribute labels to generate a multidimensional spatial retrieval index containing physical attribute labels and spatial topological attributes; initiating a query request to the topological reference storage via the data bus, retrieving physical boundary constraint parameters that match the current coordinate region of the feature points in the topological reference storage, the physical boundary constraint parameters including the corresponding allowable displacement interval matched in the corresponding dynamic feature set based on the physical attribute labels, and feeding them back to the processing unit.
[0012] Preferably, in step S4, the step of comparing the measured displacement vector of the feature point between adjacent image frames with the allowable displacement range, and adjusting the covariance weight of the feature observation component in the state observation equation according to the physical boundary constraint parameters includes: comparing the measured displacement vector of the feature point between adjacent image frames with the corresponding allowable displacement range; if the measured displacement vector deviates from the numerical range of the allowable displacement range, the feature point is determined to be a moving interference point, and the covariance weight of the corresponding feature observation component in the state observation equation is increased to reduce the weight of the moving interference point on the pose calculation.
[0013] Preferably, the steps for solving the six-DOF pose parameters of the mobile node include: constructing a factor graph optimization model with topology consistency constraints, determining the reprojection error of feature points, and determining the topology consistency evaluation factor based on physical boundary constraint parameters. According to the formula Visual observation factors are constructed and used as visual absolute constraint edges in the factor graph optimization model. A nonlinear optimization algorithm iteratively updates the state nodes in the factor graph optimization model until the global cost function containing the visual observation factors converges to the target threshold, thereby solving for the six-degree-of-freedom pose parameters of the moving node. The weighted visual observation energy function. This is the reprojection error vector of the feature points. It is a dimensionless topological consistency evaluation factor.
[0014] Preferably, the factor graph optimization model also includes an odometry pre-integration factor; the odometry pre-integration factor is constructed based on the pose transformation matrix and is used to maintain the continuity of the underlying motion state mapping of the mobile node by nonlinearly constraining the displacement increment data in adjacent sampling periods when the confidence level of the physical attribute label recognition is lower than a preset threshold, thereby ensuring the global stability of the positioning results in the case of large-area occlusion by dynamic objects.
[0015] Preferably, the method further includes the following topology benchmark storage update steps: real-time monitoring of the pose change frequency of each target in the dynamic feature set; calculating the topology change frequency within a specific coordinate region based on the pose change frequency; when the topology change frequency within a specific coordinate region exceeds 2Hz, correcting the physical motion boundary in the association rule dictionary through incremental update logic, and synchronously updating the static data table of the corresponding region in the topology benchmark storage to ensure the real-time consistency between the geometric topology model and the actual warehousing operation conditions in the physical dimension.
[0016] Preferably, the method further includes the following autonomous navigation steps: based on the calculated six-degree-of-freedom pose parameters, locate the real-time coordinates and heading angle of the moving node in the geometric topology model; extract the preset target pose, and use physical boundary constraint parameters to correct the initial path planning model to avoid fixed physical obstacle areas defined by the static feature set and temporary occupancy areas defined by the dynamic feature set; construct a motion control model based on real-time coordinates and heading angle feedback; calculate the pose residuals of the real-time coordinates and heading angles relative to the target pose; calculate the driving parameters based on the pose residuals and the corrected path planning model; map the driving parameters to the corresponding motion control commands; and output and send the motion control commands to the drive motor.
[0017] Preferably, in step S1, the displacement increment data is acquired by the encoder equipped with the underlying drive motor of the mobile node; the received displacement increment data is processed by time integration to obtain the relative displacement vector of the mobile node in the three-dimensional spatial coordinate system, and the pose transformation matrix of the mobile node in adjacent sampling periods is determined by combining the preset kinematic model.
[0018] Preferably, the original feature stream is acquired by a visual sensor array with multiple acquisition angles set at the top of the mobile node; by performing geometric feature extraction and cross-frame descriptor matching on continuous visual image frames, the three-dimensional coordinates of the feature points in the current camera coordinate system are determined, and the measured displacement vector of the same feature point between adjacent image frames is calculated for subsequent comparison with the displacement tolerance range.
[0019] Compared with existing technologies, the mobile node spatial positioning, orientation, and autonomous navigation method of this invention, oriented towards warehousing, has the following advantages:
[0020] 1. In the spatial positioning and orientation of mobile nodes, a feature verification mechanism based on the topological reference storage is established to realize the transformation of the positioning reference from probabilistic statistics to logical verification. The processor parses the initial computer-aided design geometric topology model file of the storage environment and reconstructs it into a queryable topological reference storage. By comparing the real-time sensed measured displacement vector with the preset physical boundary constraint parameters in the topological reference storage using Euclidean distance, the compliance of physical movement is determined. This mechanism blocks the influence of interference features that violate the building topology logic on pose calculation upstream of the data flow, enabling the system to identify effective spatial features according to the determined engineering design specifications, and solve the pose jump caused by the physical displacement of landmarks in the navigation system under highly dynamic environments.
[0021] 2. Achieving deep coupling between semantic tags and topological constraint boundaries enhances the navigation algorithm's adaptability to complex operating conditions. The target categories extracted by the vision module are associated with the dynamic data table in the topological reference storage. The processor dynamically rewrites the diagonal elements of the covariance matrix of the observation equation based on the legal displacement intervals of different categories of feature points in the spatial topological map. This processing method, which combines the visual perception dimension with the underlying design dimension, makes the allocation of positioning weights no longer dependent on random probability calculations of environmental changes, but rather on precise arbitration based on the physical attributes of the target to which the feature point belongs and its logical position in the warehousing operation process, effectively suppressing the interference of moving goods on global positioning.
[0022] 3. Construct a topology-consistency-driven factor graph optimization model to ensure the global robustness of the localization results. This is achieved by combining the reprojection error of visual features with the topology consistency evaluation factors returned by the retrieval. By performing a product operation to generate a reliability evaluation value constrained by the design specifications, and injecting it into the observation equation of the state estimator, this method achieves logical alignment between the visual observation factor and the odometry pre-integration factor in the nonlinear optimization iteration process. Even when faced with large-area changes in cargo stacking leading to feature loss or a decrease in semantic classification confidence, it can still maintain the global consistency of the underlying data mapping through geometric topological constraints and maintain stable orientation capabilities. Attached Figure Description
[0023] Figure 1 This is a flowchart of the mobile node spatial positioning, orientation, and autonomous navigation method for warehousing orientation provided by the present invention;
[0024] Figure 2 This is the architecture diagram of the warehouse mobile node multi-source sensing and collaborative processing system of the present invention. Detailed Implementation
[0025] The technical solutions of the embodiments of this application will be clearly described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of this application are within the scope of protection of this application.
[0026] It should be noted that all directional and positional terms used in this invention, such as: up, down, left, right, front, back, vertical, horizontal, inner, outer, top, bottom, transverse, longitudinal, center, etc., are only used to explain the relative positional relationship and connection between components in a specific state (as shown in the accompanying drawings). They are only for the convenience of describing this invention and do not require that this invention be constructed and operated in a specific orientation. Therefore, they should not be construed as limiting this invention. In addition, the descriptions of "first," "second," etc., in this invention are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated.
[0027] In the description of this invention, unless otherwise explicitly specified and limited, the terms installation, connection, and linking should be interpreted broadly. For example, they can refer to fixed connections, detachable connections, or integral connections; they can refer to mechanical connections; they can refer to direct connections or indirect connections through an intermediate medium; they can refer to the internal connection of two components. For those skilled in the art, the specific meaning of the above terms in this invention can be understood according to the specific circumstances.
[0028] In the description of this specification, references to the terms "an embodiment," "some embodiments," "illustrative embodiments," "examples," "specific examples," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example, and the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.
[0029] A method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing includes the following steps:
[0030] Step S1: The original feature stream of the warehouse scene, the physical attribute labels corresponding to the original feature stream, and the displacement increment data of the underlying drive motor are synchronously acquired through the multi-angle vision sensor array carried by the mobile node. The three-dimensional coordinate set of each feature point in the visual image frame is extracted, and the pose transformation matrix of the mobile node relative to the local coordinate system is determined according to the displacement increment data of adjacent sampling periods.
[0031] Step S2: Analyze the initial computer-aided design geometric topology model of the warehousing environment, extract the spatial distribution boundaries of shelves, aisles, pallets and goods, construct a topological benchmark storage body containing spatial constraints, define fixed shelves and aisles as static feature sets with fixed three-dimensional coordinate extreme values, define movable pallets and goods as dynamic feature sets associated with preset allowable motion displacement ranges, and establish a dictionary of association rules representing physical motion boundaries between the static feature set and the dynamic feature set.
[0032] Step S3: Construct a multi-dimensional spatial retrieval index using the three-dimensional coordinate set of feature points, and combine it with physical attribute labels to retrieve physical boundary constraint parameters that match the current coordinate region of the feature points in the topological reference storage via the data bus.
[0033] Step S4: Compare the measured displacement vectors and allowable displacement intervals of feature points between adjacent image frames, adjust the covariance weights of the feature observation components in the state observation equation according to the physical boundary constraint parameters, and combine the pose transformation matrix to complete the calculation of the six-degree-of-freedom pose parameters of the moving node.
[0034] Preferably, step S2 specifically includes: reading the geometric topology model of the storage environment and extracting boundary geometric features; using the three-dimensional coordinate set of feature points to construct the underlying architecture of a multi-dimensional spatial retrieval index, and constructing a static feature set representing the spatial positional relationship between shelves and aisles based on the boundary geometric features, and setting the motion envelope range of pallets and goods to construct a dynamic feature set; establishing an association rule dictionary representing the physical motion boundary between the static feature set and the dynamic feature set, and completing the reconstruction of the topology benchmark storage body.
[0035] Preferably, step S3 specifically includes: constructing the underlying architecture of a multidimensional spatial retrieval index using the three-dimensional coordinate set of feature points, and dividing feature points with the same attributes into independent feature clusters based on physical attribute labels; extracting the geometric center coordinates and three-dimensional envelope boundaries of the independent feature clusters, and dimensionally fusing the geometric center coordinates, three-dimensional envelope boundaries, and physical attribute labels to generate a multidimensional spatial retrieval index containing physical attribute labels and spatial topological attributes; initiating a query request to the topological reference storage via the data bus, retrieving physical boundary constraint parameters that match the current coordinate region of the feature points in the topological reference storage, the physical boundary constraint parameters including the corresponding allowable displacement interval matched in the corresponding dynamic feature set based on the physical attribute labels, and feeding them back to the processing unit.
[0036] Preferably, in step S4, the step of comparing the measured displacement vector of the feature point between adjacent image frames with the allowable displacement range, and adjusting the covariance weight of the feature observation component in the state observation equation according to the physical boundary constraint parameters includes: comparing the measured displacement vector of the feature point between adjacent image frames with the corresponding allowable displacement range; if the measured displacement vector deviates from the numerical range of the allowable displacement range, the feature point is determined to be a moving interference point, and the covariance weight of the corresponding feature observation component in the state observation equation is increased to reduce the weight of the moving interference point on the pose calculation.
[0037] Preferably, the steps for solving the six-DOF pose parameters of the mobile node include: constructing a factor graph optimization model with topology consistency constraints, determining the reprojection error of feature points, and determining the topology consistency evaluation factor based on physical boundary constraint parameters. According to the formula Visual observation factors are constructed and used as visual absolute constraint edges in the factor graph optimization model. A nonlinear optimization algorithm iteratively updates the state nodes in the factor graph optimization model until the global cost function containing the visual observation factors converges to the target threshold, thereby solving for the six-degree-of-freedom pose parameters of the moving node. The weighted visual observation energy function. This is the reprojection error vector of the feature points. It is a dimensionless topological consistency evaluation factor.
[0038] Preferably, the factor graph optimization model also includes an odometry pre-integration factor; the odometry pre-integration factor is constructed based on the pose transformation matrix and is used to maintain the continuity of the underlying motion state mapping of the mobile node by nonlinearly constraining the displacement increment data in adjacent sampling periods when the confidence level of the physical attribute label recognition is lower than a preset threshold, thereby ensuring the global stability of the positioning results in the case of large-area occlusion by dynamic objects.
[0039] Preferably, the method further includes the following topology benchmark storage update steps: real-time monitoring of the pose change frequency of each target in the dynamic feature set; calculating the topology change frequency within a specific coordinate region based on the pose change frequency; when the topology change frequency within a specific coordinate region exceeds 2Hz, correcting the physical motion boundary in the association rule dictionary through incremental update logic, and synchronously updating the static data table of the corresponding region in the topology benchmark storage to ensure the real-time consistency between the geometric topology model and the actual warehousing operation conditions in the physical dimension.
[0040] Preferably, the method further includes the following autonomous navigation steps: based on the calculated six-degree-of-freedom pose parameters, locate the real-time coordinates and heading angle of the moving node in the geometric topology model; extract the preset target pose, and use physical boundary constraint parameters to correct the initial path planning model to avoid fixed physical obstacle areas defined by the static feature set and temporary occupancy areas defined by the dynamic feature set; construct a motion control model based on real-time coordinates and heading angle feedback; calculate the pose residuals of the real-time coordinates and heading angles relative to the target pose; calculate the driving parameters based on the pose residuals and the corrected path planning model; map the driving parameters to the corresponding motion control commands; and output and send the motion control commands to the drive motor.
[0041] Preferably, in step S1, the displacement increment data is acquired by the encoder equipped with the underlying drive motor of the mobile node; the processing unit performs time integration processing on the received displacement increment data to obtain the relative displacement vector of the mobile node in the three-dimensional spatial coordinate system, and determines the pose transformation matrix of the mobile node in adjacent sampling periods by combining the preset kinematic model.
[0042] Preferably, the original feature stream is acquired by a visual sensor array with multiple acquisition angles set at the top of the mobile node; the processing unit performs geometric feature extraction and cross-frame descriptor matching on continuous visual image frames to determine the three-dimensional coordinates of the feature points in the current camera coordinate system, and calculates the measured displacement vector of the same feature point between adjacent image frames for subsequent comparison with the displacement tolerance range.
[0043] Example 1: In high-density automated warehousing operations during peak periods, a large number of pallets and goods undergo frequent spatial physical displacement with forklifts. When mobile nodes move through this environment, traditional navigation mechanisms relying on pure visual probability fields are prone to misidentifying pallet features in the operational migration state as static spatial coordinate references. This leads to continuous correlation residuals in the state observation equations, causing spatial drift and divergence in the six-degree-of-freedom pose parameter calculations of the mobile nodes. The processing unit reads the initial computer-aided design geometric topology model of the warehousing environment. Based on 3D vector primitive analysis and multi-dimensional spatial scale mapping, the processing unit traverses the internal vector layer structure of the geometric topology model level by level, extracting the vertices of each independent closed geometric contour. A three-dimensional coordinate set is used to calculate the proportional parameters of the corresponding spatial envelope and the projected area of the bottom surface. The processing unit's read-only memory stores a dictionary of storage benchmark entity dimensions. The processing unit compares the calculated proportional parameters with the storage benchmark entity dimension dictionary. When the proportional parameters of an independent closed geometric contour fall within the tolerance range of the physical dimensions of a standard heavy-duty rack, the independent closed geometric contour is defined as a rigid environmental benchmark entity. Then, the spatial distribution boundaries of the rack and aisle are extracted to construct a static feature set. Simultaneously, the spatial distribution boundaries of pallets and goods are extracted, corresponding allowable motion displacement ranges are set, and a dynamic feature set is constructed. Finally, a dictionary of association rules representing physical motion boundaries is established between the static and dynamic feature sets. A topological baseline storage volume containing absolute spatial constraints is reconstructed within the read-only memory; the physical motion boundary is a logical boundary formed by the coupling of the spatial geometric distribution of the warehousing environment and the dynamic properties of the target. For static feature sets, the physical motion boundary is determined by the absolute coordinate extrema of the shelf and aisle edges defined in the geometric topology model, forming a hard physical constraint with zero displacement redundancy; for dynamic feature sets, the physical motion boundary is generated based on the spatial distribution boundary of pallets and goods, superimposed with a preset allowable motion displacement range. The multi-angle visual sensor array carried by the mobile node acquires the original feature stream and physical attribute labels of the warehousing scene at a sampling frequency of 30Hz, based on the pixel features of the convolutional neural network. The processing unit transforms the acquired raw feature stream into a two-dimensional pixel matrix using a nonlinear mapping method. This matrix is then input into the built-in object detection weight map to perform forward tensor inference. The output consists of a set of prediction results containing candidate bounding box coordinates and object classification confidence values. The processing unit extracts feature branches from the prediction result set whose object classification confidence values exceed the baseline confirmation threshold of 0.85. It reads the bound object category code and assigns it as a physical attribute label. After completing the structured extraction of visual semantic data, the processing unit combines the displacement increment data output by the encoder of the underlying drive motor to determine the pose transformation matrix of the moving node relative to the local coordinate system and extracts the three-dimensional coordinate set of each feature point in the visual image frame.
[0044] The processing unit reads the three-dimensional coordinate set of each feature point in the visual image frame and its corresponding physical attribute label. During the traversal of the three-dimensional coordinate set, the processing unit first constructs the underlying geometric topology framework of the multi-dimensional spatial retrieval index using the feature point's three-dimensional coordinate set, and establishes the hierarchical branches of the index nodes based on the spatial clustering characteristics of the coordinate distribution. Subsequently, to introduce semantic information to enhance the directionality and determinism of subsequent retrieval, the processing unit extracts feature points with the same physical attribute label and groups them into corresponding independent feature clusters. It then accumulates the three-dimensional coordinate values of all feature points within the same independent feature cluster and divides them by the total number of feature points contained in that independent feature cluster. The processing unit calculates the geometric center coordinates and compares the coordinate values of all feature points within the independent feature cluster in three spatial dimensions. It extracts the maximum and minimum coordinate values in each spatial dimension to generate a three-dimensional envelope boundary surrounding the independent feature cluster. The processing unit combines the determined geometric center coordinates and the three-dimensional envelope boundary into a spatial topology attribute. It then concatenates the physical attribute labels with the spatial topology attribute, injecting and completing the semantic feature dimensions in the aforementioned underlying geometric topology framework, thereby generating a complete spatial retrieval index. The processing unit sends the spatial retrieval index to the topology baseline storage via the internal data bus to initiate matching. The query request for physical boundary constraint parameters utilizes a multi-dimensional spatial retrieval index constructed from the 3D coordinate set of feature points. Combined with physical attribute tags, a structured retrieval is initiated in the topological reference storage via an internal data bus to extract physical boundary constraint parameters matching the current coordinate region of the feature point. These physical boundary constraint parameters include, but are not limited to, retrieval results such as displacement tolerance intervals. The control system compares the measured displacement vector of the feature point between adjacent image frames with the feedback motion displacement tolerance interval. When the measured displacement vector deviates from the numerical range of the motion displacement tolerance interval, the processing unit determines that the feature point is a moving interference point and stores it in random access storage. The covariance weights of the corresponding characteristic observation components in the state observation equation are increased in the instrument, cutting off the transmission path of abnormal characteristic displacements to state vector updates in the matrix operation stage of nonlinear optimization iteration. Specifically, the covariance weights, as adjustment factors of the observation noise covariance matrix, are numerically inversely correlated with the confidence of the characteristic observation components. By increasing these covariance weights to improve the equivalent noise variance of the corresponding observation components, the information weight contribution of moving interference points in the global cost function is automatically reduced during the least squares solution of the factor graph optimization model, ensuring the logical robustness of the pose solution process to dynamic physical disturbances.
[0045] The control system is constructed using a factor graph optimization model constrained by topological consistency constraints. The reprojection error of feature points is calculated, and dimensionless topological consistency evaluation factors are determined based on physical boundary constraint parameters. According to the formula Construct visual observation factors; among them, The weighted visual observation energy function. This is the reprojection error vector of the feature points. This is a dimensionless topology consistency evaluation factor used to quantify the matching confidence between real-time observation features and pre-set physical constraints in the topology benchmark storage. The calculation process involves using the visual observation factor as constraint edges in a factor graph optimization model, and combining it with the pose transformation matrix to solve for the six-degree-of-freedom pose parameters of the mobile node. During nonlinear optimization, the processing unit iteratively updates the pose estimates of the state nodes by minimizing the global cost function composed of the visual observation factor and the odometry pre-integration factor until the residuals converge, thus determining the final six-degree-of-freedom pose parameters. The mobile node undergoes high-density cargo stacking reconstruction and frequent pallet crossings. During the insertion and handling operation, the control system outputs corrected three-dimensional spatial coordinates and heading angle data. It relies on underlying topology data verification to eliminate interference from dynamic targets on the state observation equations, maintaining the continuous positioning and orientation of the moving nodes in physical spaces with occlusion and changes. Based on multi-source spatiotemporal constraint cross-validation and baseline state machine filtering, the control system performs restricted baseline storage synchronization for high-frequency topology change coordinate regions. Specifically, the processing unit records the frequency of target deviations from the preset compliant trajectory by real-time comparison of the instantaneous coordinates and allowable displacement ranges of each target in the dynamic feature set. The processing unit opens a memory with a length of... A time-sliding window is used to count the total number of jumps where the pose state of each target deviates from the allowable displacement range within the window. And according to the formula The pose change frequency of each target is calculated; subsequently, the processing unit uses the formula... Calculate the frequency of topology changes within a specific coordinate region. ,in This represents the spatial weight coefficient for each target within the region, which is assigned a monotonically decreasing value based on the current Euclidean distance between the target's feature point and the moving node. This represents the total number of targets within the region. For the first in this region pose change frequency of each target Using a sliding time window, when the frequency of topology changes within a specific coordinate region exceeds the update activation extreme value of 2Hz, the processing unit determines that the region is in a highly dynamic and unstable state. Subsequently, in the factor graph optimization solution, it automatically weakens the constraint strength of the visual observation factors in that region to reduce the impact of unreliable observations on the global pose, and simultaneously triggers an incremental update procedure. The processing unit locks the static baseline rewriting operation based on single-source visual features, extracts the odometry integral track displacement data of the mobile node within 300 consecutive sampling periods, and the ambient background light intensity change rate output by the multi-angle visual sensor array. When the track displacement data confirms that the mobile node... When the body is in a mechanically stationary state with a velocity extreme value of less than 0.01 m / s and the rate of change of ambient background light intensity is lower than the light disturbance threshold of 0.05 lx / s, the processing unit releases the incremental update logic memory write permission, rewrites the static data table of the corresponding region in the topology reference storage based on the current converged visual three-dimensional coordinate parameters, and at the same time, by introducing a multi-source data cross-validation mechanism, it ensures that when the visual state observation vector is degraded due to dynamic logistics obstruction, the error transmission loop of transient spatial coordinate drift data to the underlying absolute topology reference can be cut off in time, thus ensuring the physical consistency of the data logic inside the topology reference storage.
[0046] Example 2: This example constructs a 5000-square-meter physical verification environment for warehousing, including automated shelving. The test platform is built on a differential-driven mobile chassis equipped with a physical encoder. The top of the test platform integrates a multi-angle visual sensor array with a sampling frequency of 30Hz and a spatial resolution of 0.01m. To simulate high-frequency dynamic occlusion and signal-to-noise ratio degradation interference conditions, Gaussian white noise with a signal-to-noise ratio of 20dB is injected into the test link. A dynamic logistics pallet shuttle trajectory with random occlusion properties is superimposed as a physical disturbance source. The multi-angle visual sensor array acquires the original feature stream including environmental interference. The underlying encoder... The encoder synchronously outputs odometer reference data. Regarding the selection of the trigger threshold for the covariance weight in the state observation equation, the setting of this parameter is limited by the technical trade-off between the real-time performance of pose calculation and the filtering rate of dynamic feature mismatches. When the proportion of dynamic feature space identified within the visual image frame approaches the high-density interference boundary of 50%, the excessively large tolerance range causes the pose parameters to fall into a local optimum. The trigger threshold needs to monotonically decrease to the lower limit of the displacement tolerance range as the proportion of dynamic feature space increases in order to maintain the robustness of the observation equation. Based on the constraints of the storage geometry topology and the processing computing power of the test platform, the trigger threshold of the sample group in this invention is set to 0.15m.
[0047] The test procedure sets three physical intensity gradients for the proportion of dynamic interference objects: 10%, 30%, and 50%. Simultaneously, a sample group of the present invention with a 0.15m trigger threshold and a complete topological reference storage is set up, along with a partially missing control group with a structured retrieval mechanism for missing topological reference storage, and an over-range control group with trigger thresholds set to a lower limit of 0.05m and an upper limit of 0.50m, respectively. Under the 50% high-density interference condition, the original feature stream collected by the multi-angle visual sensor array is affected by 20dB Gaussian white noise, resulting in mismatched pixels. The partially missing control group, because it resolves the moving tray features as a static coordinate reference, continuously accumulates correlation residuals during the state vector update process, resulting in an output three-dimensional spatial positioning drift of 1.25m.
[0048] The present invention receives the same original feature stream in the sample group. The processing unit extracts the three-dimensional coordinates of the feature points and initiates a structured query to the topological reference storage to extract the physical boundary constraint parameters of the corresponding shelf area. The control system compares the measured displacement vector of 0.85m with the set allowable displacement interval of 0.15m and determines that the corresponding feature point is a moving interference point. The random access memory then adjusts the covariance weight of the feature observation component in the state observation equation from the initial value of 1.0 to 100.0, so that according to the formula... The calculated visual observation factor approaches zero, among which... The weighted visual observation energy function. This is the reprojection error vector of the feature points. As a dimensionless topological consistency evaluation factor, this weight adjustment mechanism can cut off the transmission loop of abnormal feature displacements participating in state vector updates. In the evolution of the dynamic interference ratio from 10% to 50% across gradient test data, the 3D spatial positioning drift of the sample group in this invention remains within the range of 0.03m to 0.05m. The out-of-range control group, with a trigger threshold of 0.05m, exhibits a nonlinear performance inflection point at 45% dynamic interference due to excessive removal of legal static environmental features leading to under-constraint of the factor graph optimization model. The 3D spatial positioning drift increases exponentially to 0.85m. The out-of-range control group, with a trigger threshold of 0.50m, accumulates a 3D spatial positioning drift to 0.92m due to excessive dynamic noise. In summary, using a trigger threshold of 0.15m combined with physical boundary constraint parameters to adjust the covariance weights, and relying on a structured retrieval mechanism to filter out erroneous associations of dynamic scene features, can maintain stable convergence of the spatial coordinate calculation of moving nodes.
[0049] Example 3: This example combines Figures 1 to 2 This paper further explains the spatial positioning, orientation, and autonomous navigation methods for mobile nodes oriented towards warehousing, such as... Figure 1As shown, the system first executes data perception and preliminary extraction logic: It receives the input raw feature stream visual image frames, processes them using a built-in visual geometric feature extraction algorithm, and produces a set of three-dimensional coordinates for each feature point. Simultaneously, it acquires the physical attribute labels corresponding to the raw feature stream and associates these labels with the target category. These two pieces of information are then integrated into a multi-dimensional spatial retrieval index for initiating a data bus query. In another parallel data branch, the geometric topology model extracts the spatial distribution boundary, thereby constructing a topological reference storage volume. This topological reference storage volume contains both static and dynamic feature sets. Based on this, an association rule dictionary representing the constraint mapping relationship of the physical motion boundary is formed. This association rule dictionary is then used to map the physical boundary constraint parameters (including displacement tolerances). The search results (such as the allowable displacement interval) provide association indication support (as shown by the dotted line in the figure). At the same time, the multi-dimensional space search index used to initiate data bus queries also points to the physical boundary constraint parameter. After obtaining the search results, the covariance weight of the feature observation component is adjusted by comparing the measured displacement vector with the allowable displacement interval. The adjusted weight parameter is used as the observation constraint condition and input into the factor graph optimization solution model. Meanwhile, the system receives the displacement increment data provided by the underlying drive motor and uses the pose transformation matrix determined by the displacement increment data to characterize the relative pose state of the moving node. Finally, the factor graph optimization solution model combines the weight parameter and the pose transformation matrix to perform nonlinear optimization and calculate the six-degree-of-freedom pose parameters of the moving node, thereby completing the final navigation and positioning state calculation.
[0050] like Figure 2As shown, this invention also provides a multi-source perception and collaborative processing system for warehouse mobile nodes. This system includes a perception hardware terminal, an onboard core processing unit, a warehouse data support center, and a motion control execution system. The perception hardware terminal consists of a multi-angle visual sensor array, an inertial measurement unit, and underlying drive motor displacement feedback. This terminal sends raw feature stream / displacement increment data to the onboard core processing unit. The onboard core processing unit integrates multiple functional components, specifically including: a raw feature stream processing component for extracting the three-dimensional coordinates of feature points; a spatial retrieval index engine for querying physical boundary constraint parameters; an observation equation weight correction module for comparing and arbitrating displacement tolerance intervals; and a system for inputting... The system includes a factor graph optimization solution model for six-DOF pose parameters and a global spatial alignment matrix operator for synchronizing on-site calibration parameters. An electrical connection is established between the observation equation weight correction module and the factor graph optimization solution model to feed back the arbitration results of the displacement tolerance interval to the factor graph optimization solution model in real time, enabling dynamic weight adjustment during the pose calculation process. Furthermore, the warehouse data support center incorporates an initial geometric topology model and a topology reference storage. This center is responsible for providing spatial distribution boundaries and static sets of data to the onboard core processing unit. After comprehensive processing by the onboard core processing unit, the system outputs six-DOF pose parameters and navigation commands, which are then sent to the motion control execution system.
[0051] Example 4: In warehouse operations involving the mixed scheduling of standardized pallets and irregularly shaped heavy components, when mobile nodes traverse this physical space, a fixed displacement tolerance range cannot accommodate the kinematic extremes of goods of different weights. A single tolerance threshold can lead to excessive rejection of legitimate features or missed detection of abnormal interference. This error propagates to the factor graph optimization model, causing the model to receive contaminated observation components, thereby reducing the accuracy of pose parameter calculation. The processing unit introduces physical kinematic constraints based on the dynamic feature sets corresponding to different physical attribute labels to generate dynamic displacement tolerance ranges. The processing unit reads the initial computer-aided design geometric topology model and extracts the structural dimension boundaries bound to specific physical attribute labels from the initial computer-aided design geometric topology model. Simultaneously, the processing unit extracts the limit handling speed corresponding to the type of goods from the configuration parameters. And the inherent ranging error of the underlying drive motor The processing unit takes the reciprocal of the sampling frequency of the multi-angle vision sensor array to determine the sampling period Δt, and then applies the formula... Calculate the upper limit of the allowable displacement range. ,in, For maximum transport speed, The sampling period is This is the inherent ranging error. The processing unit will dynamically generate the upper limit of the allowable displacement range. In the structured retrieval stage, to quantitatively characterize the physical motion boundary, the processing unit constructs a boundary determination function in the random access memory. ,in, The real-time three-dimensional spatial coordinates of the feature points. Representing feature points and static feature sets The Euclidean distance between (e.g., shelf boundaries) The preset safety redundancy distance threshold, The measured displacement vector magnitude of the feature point. This represents the upper limit of the allowable displacement range.
[0052] The processing unit compares the measured displacement vector with the dynamically generated allowable displacement interval, adjusts the covariance weights, and constructs a factor graph optimization model. The processing unit instantiates a state node in the random access memory, which represents the six-degree-of-freedom pose parameters of the mobile node in the current image frame. The processing unit acquires the displacement increment data output by the encoder and establishes odometry relative constraint edges between adjacent state nodes. The processing unit transforms the visual observation energy function of the correlated and adjusted covariance weights into visual absolute constraint edges and connects these visual absolute constraint edges to the state nodes. The processing unit calculates the global cost function containing multi-source constraint edges based on Gauss-Newton iterative logic. The processing unit updates the values of the state nodes sequentially based on the residual descent gradient until the pose parameter changes between two adjacent iterations converge to the set termination tolerance. The system outputs an updated pose transformation matrix, which dynamically adapts to the kinematic extremes of goods of different masses. The system adjusts the upper limit of the allowable displacement interval based on characteristic physical properties, and combined with the iterative iteration of the temporal state nodes, maintains the continuous three-dimensional spatial positioning and orientation state of the mobile node in a heterogeneous cargo hybrid scheduling environment.
[0053] Example 5: When a mobile node is first deployed in a warehouse environment with unknown optical parameters, the control system triggers a field calibration procedure to establish the mapping relationship between visual pixels and physical measurement space. The execution process of the field calibration procedure includes the following steps: the mobile node moves to the physical calibration area that coincides with the origin of the topological reference storage. The multi-angle visual sensor array synchronously acquires a sequence of image frames of standard feature plates arranged in this area. The processing unit extracts the two-dimensional coordinates of the feature corner points in each image frame. Combined with the pre-stored size parameters, the intrinsic parameter matrix and distortion coefficient are calculated by minimizing the reprojection error algorithm. The control system establishes a local reference coordinate system with the geometric center of the mobile node chassis as the reference. The extrinsic parameter pose transformation matrix is calculated based on the installation offset parameters of each photosensitive component. The calibration parameter vector containing the intrinsic parameter matrix and the extrinsic parameter pose transformation matrix is written into the read-only memory.
[0054] Before routine navigation operations, the processing unit applies distortion correction and metric transformation to the original feature stream output by the multi-angle visual sensor array based on the calibration parameter vector output by the on-site calibration procedure. It maps the pixel coordinates in the image frame to a set of three-dimensional spatial coordinates in the local reference coordinate system, reads the static feature sets corresponding to the fixed shelves and aisles, calculates the spatial Euclidean distance between the three-dimensional spatial coordinate set in the local reference coordinate system and the fixed three-dimensional coordinate extrema in the topological reference storage, and generates a global spatial alignment matrix when the spatial Euclidean distance approaches the mathematical minimum convergence point. It then synchronously applies this matrix to the subsequent spatial query vector generation operation. The control system relies on the aforementioned on-site calibration procedure, which includes hardware parameters and is aligned with the spatial matrix, to construct a unified spatial retrieval index dimension benchmark, maintaining the steady-state convergence of the state observation equation update and factor graph optimization model under the physical metric boundary.
[0055] Example 6: When the system faces a situation where the raw feature stream output by the multi-angle vision sensor array has high-frequency random jitter, the control system triggers the feature observation weight quantization procedure. The processing unit extracts the measured displacement vector of the feature point in the current sampling period and calculates the Euclidean norm of the measured displacement vector to obtain the actual displacement scalar. The upper limit of the allowable displacement interval determined in the previous steps is read. And calculate the actual displacement scalar and the upper limit of the allowable displacement range. The ratio between them, within the physical boundary where the ratio is less than 1, is processed by the processing unit according to the formula. Determine the topology consistency evaluation factors ,in It is a dimensionless topological consistency evaluation factor. This is the actual displacement scalar of the feature point in the three-dimensional metric space. To correspond to the upper limit of the allowable displacement range bound to the physical attribute label, this formula quantifies the physical displacement deviation of the feature point into a confidence score within the interval [0,1] through linear mapping logic. This calculation step converts the physical displacement into a continuous linear penalty function value. The processing unit then determines the topology consistency evaluation factor. By introducing a visual observation factor construction step, the measured displacement approaches the upper limit of the allowable displacement range. The feature points obtain evaluation factors that tend to 0, and the linear mapping logic constrains the navigation system to maintain the convergence state of the state observation equation update in a physical environment with high-frequency feature jitter.
[0056] When a depth calculation blind spot occurs in the 3D coordinate set of each feature point in a visual image frame extracted by the multi-angle vision sensor array, the control system initiates a spatial coordinate intersection calculation logic based on baseline constraints. The processing unit extracts the 2D pixel coordinates of the same feature point in two adjacent vision sensor image frames, and obtains the baseline length of the spatial connection between the optical centers of the two photosensitive components by combining the extrinsic pose transformation matrix in the calibration parameter vector. Simultaneously, the processing unit calculates the spatial ray vector corresponding to the pixel coordinates by combining the intrinsic parameter matrix, and solves for the common perpendicular line segment of the two spatial ray vectors in the 3D metric space. The midpoint coordinate of the common perpendicular line segment is output as an element of the 3D coordinate set of the feature point. When the physical length of the common perpendicular line segment is greater than the inherent ranging error, the system will proceed as planned. When the processing unit determines that the feature point is an invalid spatial mapping point, the processing unit then blocks the data transmission of the feature point to the structured retrieval module and the factor graph optimization model in the random access memory. The control system relies on the underlying geometric projection logic to eliminate the divergence of the pose transformation matrix caused by abnormal spatial coordinates, and maintains the determinism of the spatial positioning and orientation process of the mobile node under the visual measurement boundary conditions.
[0057] The embodiments of this application have been described above with reference to the accompanying drawings. Unless otherwise specified, the embodiments and features in the embodiments of this application can be combined with each other. This application is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of this application without departing from the spirit of this application and the scope of protection of this invention, and all of these forms are within the protection scope of this application.
Claims
1. A method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, characterized in that, Includes the following steps: Step S1: The original feature stream of the warehouse scene, the physical attribute labels corresponding to the original feature stream, and the displacement increment data of the underlying drive motor are synchronously acquired through the multi-angle vision sensor array carried by the mobile node. The three-dimensional coordinate set of each feature point in the visual image frame is extracted, and the pose transformation matrix of the mobile node relative to the local coordinate system is determined according to the displacement increment data of adjacent sampling periods. Step S2: Analyze the initial computer-aided design geometric topology model of the warehousing environment, extract the spatial distribution boundaries of shelves, aisles, pallets and goods, construct a topological benchmark storage body containing spatial constraints, define fixed shelves and aisles as static feature sets with fixed three-dimensional coordinate extreme values, define movable pallets and goods as dynamic feature sets associated with preset allowable motion displacement ranges, and establish a dictionary of association rules representing physical motion boundaries between the static feature set and the dynamic feature set. Step S3: Construct a multi-dimensional spatial retrieval index using the three-dimensional coordinate set of feature points, and combine it with physical attribute labels to retrieve physical boundary constraint parameters that match the current coordinate region of the feature points in the topological reference storage via the data bus. Step S4: Compare the measured displacement vectors and allowable displacement intervals of feature points between adjacent image frames, adjust the covariance weights of the feature observation components in the state observation equation according to the physical boundary constraint parameters, and combine the pose transformation matrix to complete the calculation of the six-degree-of-freedom pose parameters of the moving node.
2. The method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, as described in claim 1, is characterized in that... Step S2 specifically includes: reading the geometric topology model of the storage environment and extracting boundary geometric features; constructing a static feature set representing the spatial positional relationship between shelves and aisles based on the boundary geometric features, and setting the motion envelope range of pallets and goods to construct a dynamic feature set; establishing an association rule dictionary representing the physical motion boundary between the static feature set and the dynamic feature set, and completing the reconstruction of the topology benchmark storage body.
3. The method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, as described in claim 1, is characterized in that... Step S3 specifically includes: constructing the underlying architecture of a multidimensional spatial retrieval index using the three-dimensional coordinate set of feature points, and dividing feature points with the same attributes into independent feature clusters based on physical attribute labels; extracting the geometric center coordinates and three-dimensional envelope boundaries of the independent feature clusters, and dimensionally fusing the geometric center coordinates, three-dimensional envelope boundaries, and physical attribute labels to generate a multidimensional spatial retrieval index containing physical attribute labels and spatial topological attributes; initiating a query request to the topological reference storage via the data bus, retrieving physical boundary constraint parameters that match the current coordinate region of the feature points in the topological reference storage, the physical boundary constraint parameters including the corresponding allowable displacement interval matched in the corresponding dynamic feature set based on the physical attribute labels, and feeding them back to the processing unit.
4. The method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, as described in claim 1, is characterized in that... In step S4, the step of comparing the measured displacement vector of the feature point with the allowable displacement interval between adjacent image frames and adjusting the covariance weight of the feature observation component in the state observation equation according to the physical boundary constraint parameters includes: comparing the measured displacement vector of the feature point with the corresponding allowable displacement interval between adjacent image frames; if the measured displacement vector deviates from the numerical range of the allowable displacement interval, the feature point is determined to be a moving interference point, and the covariance weight of the corresponding feature observation component in the state observation equation is increased to reduce the weight of the moving interference point on the pose calculation.
5. The method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, as described in claim 1, is characterized in that... The steps for solving the six-DOF pose parameters of the mobile node include: constructing a factor graph optimization model with topology consistency constraints, determining the reprojection error of feature points, and determining the topology consistency evaluation factor based on physical boundary constraint parameters. According to the formula Visual observation factors are constructed and used as visual absolute constraint edges in the factor graph optimization model. A nonlinear optimization algorithm iteratively updates the state nodes in the factor graph optimization model until the global cost function containing the visual observation factors converges to the target threshold, thereby solving for the six-degree-of-freedom pose parameters of the moving node. The weighted visual observation energy function. This is the reprojection error vector of the feature points. It is a dimensionless topological consistency evaluation factor.
6. The spatial positioning, orientation, and autonomous navigation method for mobile nodes oriented towards warehousing, as described in claim 5, is characterized in that... The factor graph optimization model also includes the odometry pre-integration factor; The odometry pre-integration factor is constructed based on the pose transformation matrix. It is used to maintain the continuity of the underlying motion state mapping of the mobile node by applying nonlinear constraints to the displacement increment data in adjacent sampling periods when the confidence level of the physical attribute label recognition is lower than a preset threshold.
7. The method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, as described in claim 1, is characterized in that... The method also includes the following topology benchmark storage update steps: real-time monitoring of the pose change frequency of each target in the dynamic feature set; calculation of the topology change frequency within a specific coordinate region based on the pose change frequency; when the topology change frequency within a specific coordinate region exceeds 2Hz, the physical motion boundary in the association rule dictionary is corrected through incremental update logic, and the static data table of the corresponding region in the topology benchmark storage is updated synchronously to ensure the real-time consistency between the geometric topology model and the actual warehousing operation conditions in the physical dimension.
8. The method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, as described in claim 1, is characterized in that... The method also includes the following autonomous navigation steps: based on the calculated six-degree-of-freedom pose parameters, locate the real-time coordinates and heading angle of the moving node in the geometric topology model; extract the preset target pose and use physical boundary constraint parameters to correct the initial path planning model to avoid fixed physical obstacle areas defined by static feature sets and temporary occupancy areas defined by dynamic feature sets; construct a motion control model based on real-time coordinates and heading angle feedback; calculate the pose residuals of real-time coordinates and heading angles relative to the target pose; calculate the driving parameters based on the pose residuals and the corrected path planning model; map the driving parameters to the corresponding motion control commands; and output and send the motion control commands to the drive motors.
9. The method for spatial positioning, orientation, and autonomous navigation of mobile nodes oriented towards warehousing, as described in claim 1, is characterized in that... In step S1, the displacement increment data is collected by the encoder equipped with the underlying drive motor of the mobile node; the received displacement increment data is processed by time integration to obtain the relative displacement vector of the mobile node in the three-dimensional spatial coordinate system, and the pose transformation matrix of the mobile node in adjacent sampling periods is determined by combining the preset kinematic model.
10. The spatial positioning, orientation, and autonomous navigation method for mobile nodes oriented towards warehousing, as described in claim 1, is characterized in that... The original feature stream is acquired through a visual sensor array with multiple acquisition angles set at the top of the mobile node; by performing geometric feature extraction and cross-frame descriptor matching on consecutive visual image frames, the three-dimensional coordinates of the feature points in the current camera coordinate system are determined, and the measured displacement vector of the same feature point between adjacent image frames is calculated for subsequent comparison with the displacement tolerance range.