Unpacking path planning system for high-precision laser positioning

Abstract

The invention discloses a high-precision laser positioning unpacking path planning system, and belongs to the technical field of robot automatic control and industrial automation. The system comprises a data synchronization module used for multi-sensor hardware synchronization and data alignment; the high-precision positioning module is used for outputting a precise pose based on environment skeleton characteristics and sliding window optimization; the semantic map construction module is used for fusing vision and laser data to generate a dynamic semantic grid map; the global path planning module is used for planning a smooth path in the skeleton channel by utilizing a mixed potential field improved A * algorithm; the motion control module is used for realizing closed-loop motion control and safety monitoring through model prediction and tracking; and the operation execution module is used for finishing millimeter-level precise stopping of an operation point by adopting visual servo and triggering unpacking operation. According to the method, the positioning robustness under dynamic shielding is improved through the environmental skeleton features, the safety and the high efficiency of the path are ensured by utilizing semantic understanding and intelligent planning, and the full-process automation from navigation to precise operation is realized.

Description

Technical Field

[0001] This invention relates to the field of robot automatic control and industrial automation technology, specifically to a high-precision laser positioning unpacking path planning system. Background Technology

[0002] With the development of intelligent manufacturing and smart logistics, automated unpacking operations have become a key link in improving warehouse turnover efficiency and reducing labor costs and intensity. Currently, this process mainly relies on manual labor or Automated Guided Vehicles (AGVs) based on fixed routes. The former is inefficient and inconsistent, while the latter cannot adapt to the complex needs of unpacking scenarios, such as dynamically changing package stacking, limited working space, and human-machine collaboration. To achieve flexible and intelligent automated unpacking, mobile robots need to have the ability to navigate autonomously and operate precisely in unstructured environments, which places extremely high demands on their environmental perception, positioning, planning, and control technologies.

[0003] Existing technologies suffer from the following main shortcomings: First, at the perception and localization level, traditional laser positioning methods are prone to localization drift or failure in scenarios with dense packages and temporarily obscured environmental features due to feature loss. Using only single geometric information, they lack semantic understanding of the work objects (such as packages and shelves), preventing the robot from "recognizing" the environment. Second, at the environmental modeling level, representation methods based on two-dimensional occupancy grids or cost maps cannot distinguish between different categories of objects and their states (such as fixed shelves and movable package piles), leading to path planning decisions that fail to align with operational logic. Third, at the path planning level, traditional global planning algorithms (such as the A* algorithm) often only pursue the shortest path, potentially causing the robot to graze closely around obstacles, resulting in insufficient safety redundancy. Furthermore, in dynamic environments, they lack proactive and predictive avoidance capabilities for dynamic obstacles (such as personnel and other AGVs), making it difficult to balance planning efficiency and safety. Finally, at the task execution level, the transition from navigation to task execution typically relies on coarse location arrival determination, lacking the millimeter-level precise docking and reliable action triggering mechanisms required for unpacking operations, thus impacting task success rates.

[0004] To address the aforementioned issues, there is an urgent need for a high-precision laser-based unpacking path planning system to solve the problems associated with traditional methods. Summary of the Invention

[0005] The purpose of this invention is to provide a high-precision laser positioning unpacking path planning system. It improves the positioning robustness under dynamic occlusion by using environmental skeleton features, ensures the safety and efficiency of the path by using semantic understanding and intelligent planning, and realizes full-process automation from navigation to precision operation.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A high-precision laser-guided unpacking path planning system includes: The data synchronization module, high-precision positioning module, semantic map construction module, global path planning module, motion control module, and job execution module are connected in sequence. The data synchronization module is used to perform hardware time synchronization and spatial coordinate system unification of the raw data from the lidar, vision sensor and inertial measurement unit, and output spatiotemporally aligned multimodal data frames. The high-precision positioning module is used to receive multimodal data frames, extract environmental skeleton features and perform sliding window tight coupling optimization to output the robot's high-precision global pose. The semantic map construction module is used to integrate high-precision global pose, visual semantic information and laser point cloud to construct and update a semantic occupancy grid map containing a static semantic layer and a dynamic obstacle layer in real time. The global path planning module is used to plan a smooth global path based on the semantic occupancy grid map by extracting the environmental safety skeleton, generating semantic skeleton channels, constructing a fusion potential field, and using an improved A* algorithm within the channel. The motion control module is used to generate drive commands based on global path and real-time environmental information through local trajectory planning and model prediction tracking control, and to implement closed-loop safety monitoring. The task execution module is used to switch to visual servo control mode to achieve precise docking when it reaches the vicinity of the task point, and to trigger the robotic arm to perform the unpacking task.

[0007] Furthermore, the data synchronization module specifically includes: The hardware triggering unit is used to send a unified pulse to the lidar and vision sensor to synchronize them and start data acquisition. The time alignment unit is used to assign the same timestamp to synchronously acquired laser frames and image frames, and to perform interpolation alignment on the IMU data. The spatial calibration unit is used to associate the coordinates of the laser point cloud in the laser frame with the image pixels in the image frame by applying a pre-calibrated extrinsic parameter matrix. The distortion correction unit is used to perform motion distortion correction on the laser point cloud using IMU data or odometry information, and output spatiotemporally aligned multimodal data frames.

[0008] Furthermore, the high-precision positioning module specifically includes: The geometric feature extraction unit is used to extract edge point features and planar point features from the laser point cloud of the current frame; The environmental skeleton feature extraction unit is used to identify and extract long line segments and large planes as stable environmental skeleton features from historical local maps; Two-level scanning matching units are used to first perform inter-frame matching to estimate the initial motion, and then perform keyframe-to-map matching based on environmental skeleton features to optimize and obtain the initial pose. The sliding window optimization unit is used to construct and solve an optimization problem with laser odometry constraints, skeleton feature constraints and IMU constraints as factors within a sliding window containing multiple keyframes, and output the optimized high-precision pose and confidence level.

[0009] Furthermore, the semantic map construction module specifically includes: The visual semantic segmentation unit is used to perform pixel-level segmentation of image frames using a convolutional neural network, identify the relevant categories of objects along the unpacking path, and obtain semantic segmentation results. The point cloud semantic annotation unit is used to project the semantic segmentation results onto the laser point cloud through coordinate transformation, assign a semantic label to each 3D point, and generate a semantic point cloud. The multi-layer Bayesian update unit is used to project the semantic point cloud onto the two-dimensional grid map according to the robot's pose, and to layer the grid according to different semantic categories, recursively updating the occupancy probability of each layer of grid in the form of log probability. The dynamic obstacle tracking unit is used to cluster and track multiple targets in a point cloud of obstacles labeled as dynamic, estimate their speed, and predict short-term trajectories.

[0010] Furthermore, the semantic map construction module also includes a map decay unit, which is used to gradually decay the occupancy probability of grids that have not been updated for a long time in the multi-layer Bayesian update unit, so that the map can forget the information of temporary obstacles that have been removed.

[0011] Furthermore, the global path planning module specifically includes: The accessibility determination and expansion unit is used to dynamically determine the accessibility of each semantic grid according to the current unpacking task, and to expand all impassable areas based on the robot outline and safety margin to generate a binary safety map. The topology skeleton extraction unit is used to apply a thinning algorithm to the passable area in the binary security map and extract the environmental topology skeleton with a single pixel width. The semantic skeleton channel generation unit is used to search for the shortest path connecting the starting point and the target point on the topological skeleton, and adaptively expand the width of the path based on the distance from each point to the obstacle to form a skeleton channel for limiting the search range. The fusion potential field construction unit is used to calculate the distance from each point to the nearest obstacle and the distance to the skeleton within the skeleton channel to generate a static skeleton potential field. At the same time, the time-varying threat potential field from the predicted trajectory of the dynamic obstacle is superimposed to form a comprehensive potential field.

[0012] Furthermore, the global path planning module also includes a path search and smoothing unit, which is used for: The value of the comprehensive potential field is introduced as a cost term into the cost function of the A* algorithm, and path search is performed within the skeleton channel; Extract key points from the initial path obtained from the search and remove redundant nodes; The path segments between adjacent key points are smoothly interpolated using a third-order Bézier curve to generate the final smooth global path.

[0013] Furthermore, the motion control module specifically includes: A local trajectory generator is used to sample multiple short-term trajectories in the velocity space based on the dynamic window method in each control cycle, and select the optimal trajectory that takes into account path tracking, obstacle avoidance and speed through an evaluation function. The model predictive tracking controller is used to establish an optimization problem in the finite time domain with the goal of tracking the optimal trajectory. It solves the problem online under the premise of satisfying kinematic and dynamic constraints and outputs the linear velocity and angular velocity control commands of the robot. A multi-level anomaly monitor is used to monitor location confidence, path tracking error and local planning feasibility in real time, and trigger hierarchical recovery strategies such as deceleration, emergency stop or global replanning accordingly.

[0014] Furthermore, the job execution module specifically includes: The task scene recognition and mode switching unit is used to identify the target by recognizing visual signs and switch the system control mode from path tracking to precise docking when the robot enters the preset range of the task point. A visual servo controller is used to control the visual sensors on a robot to identify feature points or planes on a target package, calculate feature space errors, and generate fine motion instructions that minimize the errors, guiding the robot to complete millimeter-level alignment. The task sequence triggering unit is used to send an orderly package of motion instructions to the robotic arm control system according to a preset process after the chassis is accurately docked and locked, and to monitor the execution status of the task.

[0015] In summary, the present invention has at least one of the following beneficial technical effects: 1. Improved robustness and accuracy of positioning in dynamic and complex environments: Hardware-level time synchronization and motion distortion correction in the data synchronization module ensure strict spatiotemporal alignment of multi-source sensor data, providing a high-quality data foundation for subsequent processing. The high-precision positioning module innovatively extracts and utilizes the inherent "skeleton features" of the environment as anchor points for global matching. Combined with sliding window tight coupling optimization, this allows the system to maintain accurate and reliable positioning even when temporarily obscuring some environmental features, effectively solving the drift problem of traditional methods in scenarios with missing features.

[0016] 2. Achieved multi-layered and intelligent environmental understanding: The semantic map construction module deeply integrates visual semantic information with laser geometric information to construct a multi-layered semantic occupancy grid map containing a static semantic layer and a dynamic obstacle layer. This map not only expresses "where you can go," but also clarifies "what is there" and "where might move," and through a mechanism for attenuating temporary occupancy information, the map can adapt to environmental changes. This provides richer and more accurate contextual information for subsequent intelligent decision-making than traditional binary maps.

[0017] 3. Safe, efficient, and semantically consistent path planning was achieved: The global path planning module extracts a safe topological skeleton based on a semantic map and generates dynamically wide "skeleton channels" to significantly reduce the search space and improve planning efficiency. By constructing a comprehensive potential field that integrates the static skeleton potential field and the dynamic threat field, and introducing it into the cost function of the improved A* algorithm, the planned path can not only actively avoid static obstacles and maintain the optimal safe distance, but also predict and avoid dynamic obstacles. Simultaneously, the accessibility of the area is dynamically determined based on the task, ensuring that the planned path conforms to the actual operational logic, thus achieving a balance between safety and efficiency, global optimization and local obstacle avoidance, and geometric constraints and semantic requirements.

[0018] 3. Ensuring the smoothness, real-time performance, and safety of robot motion: The motion control module adopts a hierarchical control architecture. The upper-level local trajectory planner combines real-time environmental information for online obstacle avoidance and speed planning, while the lower-level model predictive tracking controller ensures the robot tracks the trajectory accurately and smoothly. Combined with multi-level anomaly monitoring and recovery strategies, the system can provide graded responses to anomalies such as positioning failure and path congestion, ranging from deceleration and emergency stops to replanning. This forms a closed-loop safety barrier from perception to control, greatly improving the system's robustness in real dynamic environments.

[0019] 4. Ensures the accuracy and automation of unpacking operations: The operation execution module is designed with a mode switching process from "global navigation" to "precise docking". Through closed-loop control based on vision servoing, it directly utilizes the visual features of the target package to perform millimeter-level precise pose adjustments, solving the operation docking problem caused by insufficient accuracy of traditional absolute positioning. After docking, the system can automatically trigger the preset robotic arm operation sequence, realizing seamless connection and full-process automation from navigation to operation, significantly improving the accuracy and reliability of the operation. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of the system structure of the present invention. Detailed Implementation

[0021] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0022] like Figure 1 As shown, this invention provides a high-precision laser positioning unpacking path planning system, comprising: The data synchronization module, high-precision positioning module, semantic map construction module, global path planning module, motion control module, and job execution module are connected in sequence. The data synchronization module is used to perform hardware time synchronization and spatial coordinate system unification of the raw data from the lidar, vision sensor and inertial measurement unit, and output spatiotemporally aligned multimodal data frames. The high-precision positioning module is used to receive multimodal data frames, extract environmental skeleton features and perform sliding window tight coupling optimization to output the robot's high-precision global pose. The semantic map construction module is used to integrate high-precision global pose, visual semantic information and laser point cloud to construct and update a semantic occupancy grid map containing a static semantic layer and a dynamic obstacle layer in real time. The global path planning module is used to plan a smooth global path based on the semantic occupancy grid map by extracting the environmental safety skeleton, generating semantic skeleton channels, constructing a fusion potential field, and using an improved A* algorithm within the channel. The motion control module is used to generate drive commands based on global path and real-time environmental information through local trajectory planning and model prediction tracking control, and to implement closed-loop safety monitoring. The task execution module is used to switch to visual servo control mode to achieve precise docking when it reaches the vicinity of the task point, and to trigger the robotic arm to perform the unpacking task.

[0023] The modules mentioned above will be described in detail below: I. Data Synchronization Module 1. Input: The original 3D point cloud sequence from the lidar, where each point contains 3D coordinates and reflection intensity information; Image sequences from visual sensors (such as RGB-D cameras), including high-resolution RGB color images and corresponding depth images; A six-axis inertial data sequence from an inertial measurement unit (IMU), including three-axis acceleration and three-axis angular velocity.

[0024] 2. Output: Multimodal data frames that are strictly synchronized in time and have a unified coordinate system in space.

[0025] 3. Specific processing procedure: (1) Hardware time synchronization triggering based on hardware triggering unit The robot's main control computer sends a unified hardware trigger pulse signal to the lidar and RGB-D camera. When this pulse signal arrives, the lidar immediately starts scanning one frame, and the RGB-D camera immediately starts exposing one frame of image, ensuring that the two sensors start collecting one frame of data physically at the same time, fundamentally eliminating system errors caused by asynchronous acquisition start times. The IMU data is then output independently and continuously at its inherent high frequency. (2) Software timestamp alignment and interpolation based on time alignment units Each frame of the laser point cloud and RGB-D image acquired by hardware trigger is assigned the same trigger time timestamp (T0). For the high-frequency IMU raw data output between two consecutive synchronization times T0 and T1, based on its own precise microsecond-level timestamp, a linear interpolation method is used to normalize it to a discrete time sequence starting from T0 with a fixed time interval (e.g., 10 milliseconds), forming an IMU discrete data stream aligned with the time reference of the laser and visual data.

[0026] (3) Spatial coordinate system calibration based on spatial calibration unit Through the hand-eye calibration process, the rigid transformation relationship between the Lidar coordinate system (Lidar Frame, {L}) and the vision sensor coordinate system (Camera Frame, {C}) is accurately determined in advance, namely a rotation matrix R and a translation vector t. At the same time, through joint calibration, the transformation relationship between the robot body coordinate system (Base Frame, {B}) and the {IMU} coordinate system is determined. These transformation parameters are embedded in the system as known constants.

[0027] (4) Point cloud distortion removal operation based on distortion correction unit Since a single frame scan by a lidar takes time (typically tens of milliseconds), the points within the frame are acquired under different instantaneous poses. Using IMU pre-integrated data that traces back from time T0, or the robot velocity estimate from the previous cycle, motion distortion correction is performed on the lidar point cloud of the current frame, converting all point cloud coordinates to the {L} coordinate system at the same time T0.

[0028] Using the calibrated transformation relationship (R, t), the corrected laser point cloud in the {L} coordinate system at time T0} is projected onto the depth image plane and RGB image plane in the {C} coordinate system at time T0} through perspective projection transformation. For each laser point successfully projected into an image pixel, the association between its three-dimensional coordinates and the corresponding pixel's RGB color value and depth value is established. This completes the spatial binding of visual texture information and laser geometric information.

[0029] The T0 timestamp, the motion distortion-corrected laser point cloud, RGB image, depth image, and IMU pre-integrated data from the previous synchronization time to T0 time are packaged into a complete data frame, namely a multimodal data frame, which ensures that all information reflects the state of the robot and the environment at time T0.

[0030] II. High-precision positioning module 1. Feature extraction based on geometric feature extraction unit and environmental skeleton feature extraction unit. (1) Input: Current frame laser point cloud P from the data synchronization module current ; Local sliding window map M maintained by historical data local The map is composed of stable feature points from several past keyframes.

[0031] (2) Output: The set of local geometric features F extracted from the point cloud of the current frame current This includes edge points and planar points; The set of environmental skeleton features F identified from a local sliding window map skeleton Such as corridor walls, regular shelf columns, fixed factory structures, etc.

[0032] (3) Specific process: Point cloud filtering: for P current Voxel mesh downsampling is performed to reduce data density while preserving structure. A statistical outlier removal filter is then applied to remove discrete noise points caused by dust, sputtering, or small dynamic objects.

[0033] Curvature Calculation and Feature Classification: For each point in the point cloud, the covariance matrix is ​​calculated in its neighborhood, and the curvature is obtained through eigenvalue decomposition. Points are then classified according to the magnitude of the curvature. Edge points: Points with large curvature, usually originating from the edges of objects, corners, etc. Planar point: A point with small curvature, usually originating from large flat surfaces such as walls, floors, and tabletops; General points: Points where the curvature is centered, not used for fine registration; The threshold for determining whether the curvature is large, small, or centered can be set according to specific needs. This invention does not impose any restrictions on its value; the choice can be made based on specific requirements.

[0034] Environmental skeleton feature extraction: This is a method in M local The process of combining offline and online methods in the following context: For M local Clustering and model fitting are performed on the stable set of plane points to identify large-area planes whose positions remain unchanged during continuous observation (such as wall W1 and floor).

[0035] Boundary extraction and line segment fitting are performed on these large planes to obtain their contour lines (such as the upper and lower boundary lines of wall W1).

[0036] Define the aforementioned large plane and its long boundary line as F. skeleton These features are an inherent, long-term stable framework in the environment, providing global constraints for positioning and are insensitive to occlusion by temporarily stacked packages.

[0037] 2. Two-level scan matching and initial pose estimation based on two-level scan matching units. (1) Input: F current (Current frame features), F skeleton (Environmental skeleton characteristics); The robot's optimal pose estimate T at the previous moment prev ; (2) Output: Initial pose estimation T of the current frame init ; (3) Specific process: Motion model-based inter-frame matching: with T prev Based on this, and combined with short-term relative motion predictions provided by robot wheel odometry or IMU, a pose prediction T is obtained. predict F current Perform rapid matching with features from the previous frame (e.g., using a variant of ICP) for T. predict Fine-tuning is performed to obtain a high-frequency but potentially drifting inter-frame pose T. odom It is used to compensate for rapid movement.

[0038] Global matching based on skeleton features: with T odom As an initial guess, keyframe-to-skeleton map matching is performed, specifically: F current Plane point and F skeleton The large plane in the middle is associated (the distance from the point to the plane is calculated); F current edge points in Fskeleton Associate the long boundary lines in the middle (calculate the distance from the point to the line); Construct and minimize the sum of these distance errors, and use nonlinear optimization to find the optimal pose T that fits the current frame features onto the environment skeleton. init This is used to effectively correct the cumulative drift of inter-frame matching by utilizing global stability features.

[0039] 3. Sliding window tight coupling optimization based on sliding window optimization unit (1) Input All poses [T] within a sliding window (such as the most recent 10 keyframes) including the current frame. k T k-1 , ..., T init ]; Feature point clouds corresponding to each frame within the sliding window; Pre-integral constraints from the IMU; (2) Output The optimal poses of all keyframes within the sliding window after global optimization, especially the optimized pose T of the current frame. optimized ; (3) Specific process Constructing a factor graph: Modeling the optimization problem as a factor graph: Nodes: Variables to be optimized, i.e., the poses of each keyframe within the sliding window.

[0040] Edges (constraint factors): Laser odometry factor: Relative pose constraints generated between adjacent keyframes through inter-frame matching; Skeleton feature factor: The constraint between the pose of each keyframe and its observed skeleton features (planes, lines). This is the key to tight coupling, which directly injects global skeleton information into the optimization framework. IMU pre-integration factor: The relative motion constraint obtained by integrating IMU measurements between adjacent keyframes, providing high-frequency attitude change information; Closed-loop factor: If a successful match is detected between the current frame and a historical non-adjacent frame, a strong constraint is added to correct the drift.

[0041] Nonlinear optimization solution: Using optimization algorithms such as Levenberg-Marquardt, the values ​​of all pose nodes are adjusted to minimize the overall error of all factor constraints on the graph.

[0042] Marginalization and Window Update: After optimization, the oldest frame in the window and its constraints are marginalized, and its information is preserved in a priori form. At the same time, a new frame is added to maintain a constant window size, thereby achieving continuous and recursive optimization.

[0043] 4. Finally, based on the positioning quality assessment and output unit, the positioning quality is assessed and output. (1) Input Optimized pose T optimized ; Residual and covariance information calculated during the optimization process; (2) Output Final pose T final ; The confidence score C for this localization (e.g., inlier rate based on skeleton feature matching, optimized residual size, etc.); (3) Specific process: Confidence calculation: Analyze the optimization results. For example, if the number of inliers that successfully match the skeleton features in the current frame is large and evenly distributed, and the residual after optimization is small, then the confidence C is high; if severe occlusion results in fewer matching skeleton features, then C is low.

[0044] Decision-making and output: If C is higher than the threshold, then T is output directly. optimized As T final ; If C is below the threshold, the system determines that the positioning may be unreliable, and can adopt two strategies: a) Trigger a warning and attempt to perform a short-term dead reckoning using the wheel odometer; b) Based on T optimized And low-confidence information, instructing subsequent path planning modules to adopt a more conservative strategy (such as slowing down or searching for a safer path).

[0045] Feature map update: based on T final Add stable feature points in the current frame that are not associated with dynamic objects to the local sliding window map M. local In China, the representation of the environment continues to be enriched.

[0046] III. Semantic Map Construction Module 1. Point cloud semantic annotation based on visual semantic segmentation units and point cloud semantic annotation units (1) Input: High-confidence optimized pose T of the current frame from the high-precision positioning module final ; RGB image I from the data synchronization module, synchronized with the current frame time rgb and the corresponding depth image I depth ; The current frame laser point cloud P, already aligned with the image, from the data synchronization module. calibrated ; A pre-trained lightweight semantic segmentation neural network model (such as MobileNetV3+DeepLabV3 Lite, which is not limited in this invention, as long as it can achieve the functions described in this invention) is trained on a logistics scenario dataset containing categories such as "fixed shelves", "pallets", "package stacks", "pedestrians", "AGVs", "ground", and "undefined obstacles".

[0047] (2) Output: 3D semantic point cloud P of the current frame with semantic labels semantic Each point contains: three-dimensional coordinates (x, y, z), intensity value, and a defined semantic category label.

[0048] (3) Specific process: Visual semantic segmentation: Transforming RGB images into I rgb Input a semantic segmentation neural network, and the network outputs a pixel-level semantic segmentation map I with the same resolution as the input image. label I label The value of each pixel represents the object category that the location is most likely to belong to.

[0049] Coordinate transformation and label projection: Utilizing the camera-LiDAR calibration parameters already completed by the data synchronization module, the LiDAR point cloud P is transformed... calibrated Each 3D point in the image is projected onto image I. label In the pixel coordinate system, for points that are successfully projected into the image range, the semantic label of the pixel where the projection is located is directly assigned to the laser point. For points projected outside the image range (such as the part where the LiDAR field of view is wider than the camera's), the label is temporarily marked as "unlabeled".

[0050] Deep-based tag verification and patching: To improve reliability, consistency checks are performed. Calculate the three-dimensional coordinates of the laser point and the corresponding pixel depth value I. depth If the distance between the inversely calculated 3D coordinates is too large (exceeding the set threshold, such as 5cm), the projection matching is considered unreliable, which may be due to the movement of dynamic objects or small registration errors. The label of this point is temporarily set to "pending". For "unlabeled" and "pending" points, a method based on point cloud neighborhood statistics is used for label repair: check the K nearest successfully labeled neighboring points, and if the proportion of a certain semantic label among them exceeds a certain percentage (e.g., 70%), then assign that label to the current point. Otherwise, mark it as "unknown obstacle".

[0051] Output semantic point cloud: After labeling all points, generate the semantic point cloud P for the current frame. semanticThis point cloud not only describes the geometry of the environment, but also gives each point a clear physical meaning.

[0052] 2. Construction and updating of semantic occupancy raster map based on multi-layer Bayesian update unit and map decay unit. (1) Input Current frame semantic point cloud P semantic ; Current frame pose T final ; A global, up-to-date two-dimensional multi-layer semantic occupancy raster map M semantic_grid Each layer of the map corresponds to a semantic category (such as "ground layer", "shelf layer", "package layer", "dynamic obstacle layer"), and each grid cell stores two core states: log. odds Occupy probability logarithm and age last observation time frame.

[0053] (2) Output Updated global semantic occupancy grid map M semantic_grid .

[0054] (3) Detailed process: Point cloud rasterization and projection: using T final The semantic point cloud P of the current frame semantic All points are transformed from the sensor coordinate system to the global world coordinate system. Then, these 3D points are vertically projected onto a 2D horizontal plane (ground plane), and their corresponding raster index (i, j) is determined based on their global 2D coordinates (X, Y).

[0055] Bayesian probability update: For each hit raster, update the probability on the layer corresponding to its semantic category. Recursive updates are performed using the logarithmic probability form of a binary Bayesian filter. For layers that match the point cloud semantic label L: the raster's log odds The value is increased by a positive observation value log(p / (1-p)), where p is the probability of hitting the observation (e.g., 0.7). For all other semantic layers: since the location is observed to be occupied by category L, this provides counter-evidence for other categories, and the logarithm of the corresponding raster for these layers... odds The value is increased by a negative observation (i.e., decreased by a fixed value). At the same time, update the age of this raster in all layers to the current time frame.

[0056] Empty region update and dynamic decay: For grids within the current sensor field of view (FOV) but not hit by any point cloud, perform an "empty" update: logs of all layers oddsThe value is reduced by a fixed value (indicating stronger evidence that the position is empty); Introducing a dynamic decay mechanism: Traversing the map, for grid cells whose age exceeds a certain number of frames (e.g., N=50 frames) and have not been updated, their log... odds The value will slowly decay back to its initial value over time. This allows the map to gradually "forget" objects that have not appeared for a long time (possibly because they have been moved), especially information in the "wrapping layer" and "dynamic obstacle layer".

[0057] Map state extraction: At any given time, the log of each raster across all layers can be used to extract the map state. odds Value, parsing out the final state: Static occupancy: If the log of static layers such as "shelf layer" or "ground layer" is occupied. odds If the value remains above the threshold, the grid is considered to be occupied by a static object. Semantic occupancy: if the log of the "wrapping layer" odds If the value is the highest and exceeds the threshold, the grid is considered to be occupied and its height characteristics are recorded (statistics from the original 3D point cloud). Free space: If the log of all layers odds If all values ​​are below the low threshold, the grid is considered free. Dynamic / Temporary Occupation: If the log of the "Dynamic Barrier Layer" or "Wrapping Layer" is... odds If the value is high but the age is new, and the static layer value is low, it is marked as temporary occupation.

[0058] 3. Dynamic obstacle layer maintenance and prediction based on dynamic obstacle tracking unit (1) Input Current frame semantic point cloud P semantic Dynamic point sets labeled "pedestrian" and "AGV"; The pose of the current frame and several previous frames; The semantic occupancy of the raster map M at the current moment semantic_grid Dynamic obstacle layer in; (2) Output Dynamic obstacle list D list It includes the estimated position, velocity, bounding box, and predicted short-term trajectory of each dynamic obstacle.

[0059] (3) Specific process: Dynamic point clustering and tracking: Euclidean clustering is performed on all dynamic category points in the current frame to form multiple point clusters. Each point cluster represents a potential dynamic obstacle. Using a multi-object tracking algorithm (such as Kalman filtering or a simpler IOU tracker), the point clusters detected in the current frame are associated with the dynamic obstacle trajectories maintained in the previous frame.

[0060] State estimation and map update: For successfully associated trajectories, their state (center position, velocity) is updated using a Kalman filter. Their current position and estimated occupied area (based on the bounding rectangle of the point cluster) are then updated to M. semantic_grid The "dynamic obstacle layer" and the log of that area odds Set the value to a higher value, and the age to the latest value.

[0061] Short-term trajectory prediction: Based on the currently estimated velocity and position, predict the trajectory of each active dynamic obstacle within a short future timeframe (e.g., 1-3 seconds). Store the predicted trajectories as time series data in D. list middle.

[0062] Anomaly Handling: For unmatched historical trajectories, continue prediction for 1-2 frames. If there are multiple consecutive unmatched frames, remove the trajectory from the active list and initiate a decay process for that area in the map's "Dynamic Obstacle Layer".

[0063] IV. Global Path Planning Module 1. Semantic map preprocessing and skeleton extraction based on drivability determination, expansion units, and topological skeleton extraction units. (1) Input: The latest semantic occupancy raster map M from the semantic map building module semantic_grid It contains multi-layered semantic information and dynamic obstacle locations; The robot's physical outer radius R robot and a preset additional safety margin δ; The current unpacking task description includes the coordinates of the target work point and the work type (such as "full stack pickup" or "single item picking").

[0064] (2) Output: Binary safety-expanded map M for path search binary (Passable areas are 1, impassable areas are 0); From M binary Extracted environmental topology skeleton map M skeleton It consists of a series of center lines with a width of one pixel.

[0065] (3) Specific process: Semantic information fusion and accessibility determination: Based on the task, dynamically generate "accessibility determination rules" for the current frame. This invention provides an embodiment, specifically: Static layer: Fixed shelves and walls are always impassable; Dynamic layer: The current location of dynamic obstacles (personnel, other AGVs) is impassable; Operation semantic layer: The accessibility of the parcel stack area is dynamically determined based on the task: if it is "full stack pickup", the area near the target stack is considered impassable before the operation (to prevent collisions) and is considered accessible after the operation; the passage area is always a high-priority accessible area.

[0066] Safety distance inflation: To ensure the robot body does not collide with any obstacles, for all grids determined to be impassable (including static and dynamic obstacles), a safety distance inflation is applied, centered on the grid and extending outwards by a radius of R. inflate =R robot A morphological dilation operation (such as disk dilation) is performed with a radius of +δ. The dilated area is marked as impassable (0), and the remaining areas are marked as passable (1), generating a binary map M. binary This involves transforming the robot's dimensions into a path planning problem for a point robot.

[0067] Environmental skeleton extraction: applying a refinement algorithm to M binary The algorithm iteratively removes the boundary points of these passable regions (regions with a value of 1) until all regions are reduced to a single-pixel-wide centerline, while preserving the original regions' topological connectivity. This process generates the environment skeleton M. skeleton It represents the "central hub" of all safe passages in the environment. It is located as far away as possible from the boundaries of the expanding obstacles on both sides, and naturally has a high level of security.

[0068] Skeleton optimization: Post-processing of the extracted initial skeleton includes: removing isolated branches (burrs) that are too short; smoothing the jagged skeleton while ensuring connectivity; and storing key intersections and endpoints as topology nodes.

[0069] 2. Generate skeleton channels based on semantic skeleton channel generation units (1) Input: Skeleton Map M skeleton ; Binary Security Map M binary ; Robot current pose P current and the target point P goal ; The semantic context at the target point, such as: the target is "package pile A".

[0070] (2) Output: A restricted skeleton channel region connecting the start and end points is represented as a set of grid cells; The basic passage cost C for each grid cell within the skeleton channel base (i, j); (3) Specific process: Connection between start / end point and skeleton: in Mskeleton Find the distance to P current and P goal The nearest skeletal point S start and S goal Ensure P is connected. current To S start and P goal To S goal The line segment at M binary It is perfectly feasible in China.

[0071] Initial skeleton path search: on skeleton map M skeleton Above, use Dijkstra's algorithm to search from S start To S goal The shortest path (at the cost of skeleton points). This path is a polyline L consisting of a series of skeleton points. skeleton It is the center line of the global security channel.

[0072] Generate dynamic skeleton channels: using skeleton path L skeleton Based on this, a dynamically wide channel region is generated. For L skeleton For each point on the graph, calculate its distance d to the nearest inflated obstacle boundary. safe The width of the channel at this point is defined as W = 2 * β * d safe In the formula, β is the channel coefficient, for example, 0.8, which ensures that the channel is completely within the safe area. The width area of ​​all points is taken as the union to obtain the final skeleton channel, which greatly reduces the search space from the entire map to a strip area around the safe center line, thus greatly improving the search efficiency.

[0073] Assigning a semantic basis cost: Traverse each grid within the Channel and assign a basic passage cost C based on the original semantic information (before expansion). base The guide planner prioritizes routes with higher operational efficiency, while ensuring safety.

[0074] 3. Cost function for constructing the fused skeleton potential field and dynamic threat field based on the fused potential field construction unit. (1) Input: Skeleton Channel; Skeleton Map M skeleton ; Binary Security Map M binary (Including the area of ​​dynamic obstacle expansion); Dynamic obstacle prediction trajectory list D list ; (2) Output: The combined potential value P of each grid cell within the skeleton channel total (i, j); The improved A* algorithm cost function is F(n) = g(n) + h(n) + λ*P. total (n); (3) Detailed process: Calculate the static skeleton potential field: For each grid cell (i, j) within the channel, calculate its distance d to the nearest expanding obstacle. o And to the nearest skeleton point M skeleton distance d s Calculate the static skeleton potential field P. s (i, j) = [1-d o / (d o +d s )] 2 The potential field is smallest (safest) on the skeleton, largest (most dangerous) on the surface of the obstacle, and transitions smoothly within the channel.

[0075] Calculate the dynamic threat potential field: For each predicted dynamic obstacle trajectory point, generate a local, time-varying repulsive potential field P at its estimated future location. d The magnitude of this potential field is inversely proportional to the time difference between the robot's expected arrival at that location (the closer the time, the greater the threat), and inversely proportional to the distance. The total dynamic threat potential field of a grid within the channel is the P generated by all dynamic obstacles. d The superposition of.

[0076] Potential field fusion and normalization: Synthetic potential field P total =ω s *P s +ω d *P d , where ω s and ω d This is a weighting coefficient, which can be adjusted according to the dynamic nature of the environment (increase ω when there are many dynamic objects). d ). P total Normalize to the interval [0, 1].

[0077] Define the improved cost function: The total cost F(n) of the A* algorithm consists of three parts: g(n): The actual cost from the starting point to node n, accumulated C base (Semantic cost) and travel distance; h(n): Heuristic cost from node n to the target point (e.g., Euclidean distance); λ*P total (n): Potential field penalty term, λ is the potential field weight coefficient, when P total (n) Exceeding the corresponding safe distance d safe When the threshold is reached, it can be set to infinity to disable searching.

[0078] 4. Intra-channel A* search and path post-processing based on path search and smoothing units. (1) Input: Skeleton Channel; Improved cost function F(n); Starting point P current and target point P goal .

[0079] (2) Output: An initial optimized path consisting of a series of waypoints. raw ; (3) Specific process: Restricted A* Search: Within the set of grid cells defined by the skeleton channel, the standard A* algorithm is run, with the open and closed sets containing only the grid cells within the channel. An improved cost function F(n) is used for evaluation each time a node is expanded. This ensures that the searched path is not only shorter but also automatically tends to traverse the center of the channel and avoids dynamic threats.

[0080] Key point extraction: Remove Path raw Collinear redundant nodes are retained, with only critical turning points where the path direction changes significantly being preserved.

[0081] Third-order Bézier curve smoothing: To meet the requirements of continuous robot motion, third-order Bézier curves are used to smooth the path segments between adjacent key points. This ultimately generates a smooth, differentiable global path that the robot can execute smoothly. global .

[0082] V. Motion Control Module 1. Local trajectory generation and velocity planning based on a local trajectory generator (1) Input: Global path from the global path planning module global ; Kinematic model of a robot; Robot dynamics constraints: maximum / minimum linear velocity [v] min v max Maximum / minimum angular velocity [ω] min ω max Maximum acceleration [a] max and maximum angular acceleration [α] max ]; Real-time dynamic obstacle list D list And its predicted trajectory.

[0083] (2) Output: A timestamped local trajectory that satisfies motion constraints (Trajectory) local .

[0084] (3) Detailed process: Path resampling and parameterization: for Path global Resampling with a constant arc length or a constant time interval yields a denser set of path points. Using the path arc length s as a parameter, cubic spline interpolation is performed on the path points to obtain smooth functions x(s), y(s), and θ(s) with respect to s, which can then be used to calculate the curvature κ(s) at any point on the path.

[0085] Dynamic window-based velocity planning: In each planning cycle (e.g., 0.1 seconds), the robot's current state (v) is used as the reference. current ω current Based on this, a set of velocity pairs (v, ω) satisfying acceleration constraints are sampled in the velocity space (v, ω). i ω i For each sampling rate pair: The trajectory of the simulated robot after moving at this speed for a short time interval Δt; Evaluation function calculation: Calculates the evaluation score for the simulated trajectory, which is composed of multiple weighted components: Goal orientation: The degree of proximity between the trajectory endpoint and the local target point (the point at Lookahead_distance on the path); Path fit: Trajectory and global reference path global The average deviation; Speed ​​preference: Encourages moving at higher speeds; Dynamic obstacle avoidance: Check if the trajectory matches D list If the predicted trajectory of a dynamic obstacle conflicts in time and space, the score for this item will be negative infinity, and the speed pair will be directly rejected. Static safety: Checks whether the trajectory collides with static obstacles in the real-time semantic map.

[0086] Optimal velocity selection and trajectory generation: From all unrejected sampled velocity pairs, select the one with the highest evaluation score (v). opt ω opt This serves as the instruction for the next control cycle. Based on (v) opt ω opt Using the robot's kinematics model, the local predicted trajectory for the next N control cycles is generated through integration. local , which serves as the reference input for the controller.

[0087] 2. Model predictive tracking control based on model predictive tracking controller (1) Input: Local reference trajectory local ; High-precision real-time pose feedback Pose feedback and its confidence level C; Robot kinematics model and constraints.

[0088] (2) Output: The final control command sent to the robot's drive motor: target speed v of the revolver. l_cmd Right wheel target speed v r_cmd ; (3) Specific process: Construct the optimization problem: In each control cycle, pose the current position. feedback Given an initial state, establish a model predictive control problem with a finite time domain (e.g., 2 seconds). The optimization variables are a series of control variables (v, ω) in the future time domain.

[0089] Design the objective function: The objective function aims to minimize: Tracking error: Predicted trajectory vs. local reference trajectory local Deviation in pose; Control quantity change: Drastic changes in speed command between adjacent control cycles to ensure smooth motion; Terminal error: The deviation between the predicted terminal state in the time domain and the terminal state of the reference trajectory.

[0090] Imposing constraints: Applying hard constraints to an optimization problem to ensure that the solution satisfies: Kinematic constraints: v min ≤v≤v max ω min ≤ω≤ω max ; Dynamic constraints: |v k -v {k-1} | / Δt≤a max ,|ω k -ω {k-1} | / Δt≤α max ; Obstacle avoidance constraint (optional hard constraint): All points on the predicted trajectory must be within the traversable area of ​​the semantic map.

[0091] Real-time solution and output: Solve the constrained optimization problem online (usually transformed into a quadratic programming problem) to obtain the optimal control sequence. Extract the first control variable (v*, ω*) from this sequence and inversely calculate the left and right wheel speed commands (v*, ω*) based on the kinematic model. l_cmd v r_cmd And send it to the driver.

[0092] 3. Multi-level anomaly monitoring and recovery strategy based on multi-level anomaly monitors (1) Input: Pose confidence level C; Controller tracking error e track ; Does the local planner find an effective speed pair for the flag? DWA ; Information on sudden obstacles in real-time semantic maps.

[0093] (2) Output: System status indicators: NORMAL, CAUTION, ESTOP, or RECOVERY; Recovery policy instructions corresponding to the system status identifier.

[0094] (3) Specific process: Status monitoring: Continuously monitor the following key metrics: C <C th_low The location accuracy is too low, and the location may fail. e track >e th_max The tracking error is too large, and the path is seriously deviated. Flag DWA ==False: The local planner cannot find collision-free velocity pairs and may be blocked by dynamic obstacles or get stuck in a dead zone. An unexpected static obstacle suddenly appeared ahead.

[0095] Tiered response strategy: CAUTION (Caution): When e track When C increases or decreases but does not exceed the threshold, the controller increases the weight of the tracking error, and the robot automatically decelerates. ESTOP (Emergency Stop) state: When the Flag DWA ==False (impending collision) or C drops suddenly, immediately send a zero-speed command and the robot will brake urgently. RECOVERY state: Upon triggering, the following process is executed: a. Localization recovery: If localization fails, control the robot to slowly rotate in place or attempt small movements to obtain new environmental features and regain high-confidence localization. b. Path replanning: If the environment is congested, the robot's current position is set as the new starting point. Combined with the updated semantic map, global path planning is retried to find an alternative path. c. Manual confirmation: If the autonomous recovery fails, an anomaly is reported and remote operator intervention is awaited.

[0096] State transition: Based on the monitoring results and the effect of recovery actions, the system safely transitions between the above states, forming a complete fault-tolerant autonomous chain.

[0097] VI. Job Execution Module 1. Work area approximation and mode switching based on the work scene recognition and mode switching unit. (1) Input: Global path endpoint P goal (Entrance point to the work area); The precise outline and feature point information of the target package stack in the semantic map; The robot's current pose.

[0098] (2) Output: Dock mode switching command; Vision-based fine-grained local navigation target P fine .

[0099] (3) Specific process: Area arrival determination: When the robot's center of mass is close to P goal The distance is less than the threshold D approach If the speed is less than 1 meter (e.g., 1 meter) and the speed is below the threshold, it is determined that the operation has entered the approach zone.

[0100] Operation scene recognition: Through visual sensors, the system identifies preset operation scene markers (such as QR codes or ArUco codes) or directly identifies specific visual features of the target package pile to confirm that the target is correct.

[0101] Mode switching: The navigation system switches from global path tracking mode to "precise docking mode at work points". In this mode, the global path tracking controller is disabled, and the docking controller designed for low speed and high precision is enabled.

[0102] 2. Precise positioning and docking of vision servoing based on vision servo controller (1) Input: The desired features of the target package stack in the robot coordinate system, such as the three-dimensional coordinates P of a specific marker point on the package. mark_desired , or the normal vector n of the enclosing plane desired ; Real-time extraction of current features P by visual sensors mark_current or n current ; Laser positioning and orientation.

[0103] (2) Output: Visual servo control commands for fine alignment (v vision ω vision ).

[0104] (3) Specific process: Feature extraction and matching: Using an RGB-D camera, visual features (such as SIFT and ORB feature points) or known geometric markers on the target package are detected in real time. Through feature matching or template matching, the pixel position of the feature in the current image is calculated, and combined with depth information, its three-dimensional coordinates P in the robot camera coordinate system are calculated. mark_current Simultaneously, it can calculate the point cloud fitting normal vector n of the main plane. current .

[0105] Error calculation: Construct the image / feature space error e. For example, for positional alignment, the error e = P. mark_current -P mark_desired For attitude alignment, the error may be related to the angle between the normal vectors.

[0106] Visual servoing control law: Image-based or position-based visual servoing is employed. The controller calculates the required robot end effector (or chassis) speed (v) based on the error e. vision ω vision This allows the error to approach zero. This control law is directly integrated into the model predictive controller as the tracking target for the current cycle; that is, the controller target is no longer the path point, but rather minimizing the feature error.

[0107] Iterative alignment: The robot moves slowly, and the visual servo closed loop continues to work until the norm of the error e is less than the set millimeter-level threshold (e.g., |e|<5mm) and remains stable for more than a certain period of time (e.g., 1 second).

[0108] 3. Status locking and work instruction triggering are performed based on the task sequence triggering unit. (1) Input: Visual servoing error e and its steady state; Robot's current pose and velocity; The pre-set unpacking process flow.

[0109] (2) Output: Robot chassis locking command; The task instruction package is sent to the robotic arm control system.

[0110] (3) Specific process: Docking confirmation and locking: Once precise alignment is complete and the robot is completely stationary, a docking completion signal is issued. The chassis controller switches to position holding mode, attempting to maintain the current position even under minor disturbances.

[0111] Task sequence triggering: The central controller sends instruction packets to the robotic arm control system in sequence according to the task type. For example: Command 1: POWER_ON_AND_HOME (Robot arm powers on and returns to zero); Command 2: MOVE_TO_PRE_GRASP_POSE (Move to the pre-capture pose); Command 3: EXECUTE_CUTTING_TRAJECTORY (Execute cutting trajectory); Command 4: GRASP_AND_LIFT (Fetch and lift).

[0112] Operation process monitoring: The robot chassis remains locked during robot arm operation. Robot arm status feedback and visual monitoring ensure the operation proceeds according to the procedure. If the operation fails (e.g., gripper detaches), a retry process can be triggered or an anomaly can be reported.

[0113] Task completion and reset: After receiving the robot arm's task completion signal, the chassis is unlocked and the recovery state is triggered. Typically, a path is planned to return from the current point to the charging area or the next task point. The system switches back to global path tracking mode and starts a new task cycle.

[0114] Embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0115] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0116] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1The function specified in one or more boxes.

[0117] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0118] Contents not described in detail in this specification are prior art known to those skilled in the art. It is hereby indicated that the above description is intended to help those skilled in the art understand this invention, but does not limit the scope of protection of this invention. Any equivalent substitutions, modifications, improvements, or simplifications of the above descriptions that do not depart from the essential content of this invention fall within the scope of protection of this invention.

Claims

1. A high-precision laser-positioned unpacking path planning system, characterized in that, include: The data synchronization module, high-precision positioning module, semantic map construction module, global path planning module, motion control module, and job execution module are connected in sequence. The data synchronization module is used to perform hardware time synchronization and spatial coordinate system unification of the raw data from the lidar, vision sensor and inertial measurement unit, and output spatiotemporally aligned multimodal data frames. The high-precision positioning module is used to receive multimodal data frames, extract environmental skeleton features and perform sliding window tight coupling optimization to output the robot's high-precision global pose. The semantic map construction module is used to integrate high-precision global pose, visual semantic information and laser point cloud to construct and update a semantic occupancy grid map containing a static semantic layer and a dynamic obstacle layer in real time. The global path planning module is used to plan a smooth global path based on the semantic occupancy grid map by extracting the environmental safety skeleton, generating semantic skeleton channels, constructing a fusion potential field, and using an improved A* algorithm within the channel. The motion control module is used to generate drive commands based on global path and real-time environmental information through local trajectory planning and model prediction tracking control, and to implement closed-loop safety monitoring. The task execution module is used to switch to visual servo control mode to achieve precise docking when it reaches the vicinity of the task point, and to trigger the robotic arm to perform the unpacking task.

2. The high-precision laser positioning unpacking path planning system according to claim 1, characterized in that, The data synchronization module specifically includes: The hardware triggering unit is used to send a unified pulse to the lidar and vision sensor to synchronize them and start data acquisition. The time alignment unit is used to assign the same timestamp to synchronously acquired laser frames and image frames, and to perform interpolation alignment on the IMU data. The spatial calibration unit is used to associate the coordinates of the laser point cloud in the laser frame with the image pixels in the image frame by applying a pre-calibrated extrinsic parameter matrix. The distortion correction unit is used to perform motion distortion correction on the laser point cloud using IMU data or odometry information, and output spatiotemporally aligned multimodal data frames.

3. The high-precision laser positioning unpacking path planning system according to claim 2, characterized in that, The high-precision positioning module specifically includes: The geometric feature extraction unit is used to extract edge point features and planar point features from the laser point cloud of the current frame; The environmental skeleton feature extraction unit is used to identify and extract long line segments and large planes as stable environmental skeleton features from historical local maps; Two-level scanning matching units are used to first perform inter-frame matching to estimate the initial motion, and then perform keyframe-to-map matching based on environmental skeleton features to optimize and obtain the initial pose. The sliding window optimization unit is used to construct and solve an optimization problem with laser odometry constraints, skeleton feature constraints and IMU constraints as factors within a sliding window containing multiple keyframes, and output the optimized high-precision pose and confidence level.

4. The high-precision laser positioning unpacking path planning system according to claim 3, characterized in that, The semantic map construction module specifically includes: The visual semantic segmentation unit is used to perform pixel-level segmentation of image frames using a convolutional neural network, identify the relevant categories of objects along the unpacking path, and obtain semantic segmentation results. The point cloud semantic annotation unit is used to project the semantic segmentation results onto the laser point cloud through coordinate transformation, assign a semantic label to each 3D point, and generate a semantic point cloud. The multi-layer Bayesian update unit is used to project the semantic point cloud onto the two-dimensional grid map according to the robot's pose, and to layer the grid according to different semantic categories, recursively updating the occupancy probability of each layer of grid in the form of log probability. The dynamic obstacle tracking unit is used to cluster and track multiple targets in a point cloud of obstacles labeled as dynamic, estimate their speed, and predict short-term trajectories.

5. The high-precision laser positioning unpacking path planning system according to claim 4, characterized in that, The semantic map construction module also includes a map decay unit, which is used to gradually decay the occupancy probability of grids that have not been updated for a long time in the multi-layer Bayesian update unit, so that the map can forget the information of temporary obstacles that have been removed.

6. The high-precision laser positioning unpacking path planning system according to claim 5, characterized in that, The global path planning module specifically includes: The accessibility determination and expansion unit is used to dynamically determine the accessibility of each semantic grid according to the current unpacking task, and to expand all impassable areas based on the robot outline and safety margin to generate a binary safety map. The topology skeleton extraction unit is used to apply a thinning algorithm to the passable area in the binary security map and extract the environmental topology skeleton with a single pixel width. The semantic skeleton channel generation unit is used to search for the shortest path connecting the starting point and the target point on the topological skeleton, and adaptively expand the width of the path based on the distance from each point to the obstacle to form a skeleton channel for limiting the search range. The fusion potential field construction unit is used to calculate the distance from each point to the nearest obstacle and the distance to the skeleton within the skeleton channel to generate a static skeleton potential field. At the same time, the time-varying threat potential field from the predicted trajectory of the dynamic obstacle is superimposed to form a comprehensive potential field.

7. The high-precision laser positioning unpacking path planning system according to claim 6, characterized in that, The global path planning module also includes a path search and smoothing unit, which is used for: The value of the comprehensive potential field is introduced as a cost term into the cost function of the A* algorithm, and path search is performed within the skeleton channel; Extract key points from the initial path obtained from the search and remove redundant nodes; The path segments between adjacent key points are smoothly interpolated using a third-order Bézier curve to generate the final smooth global path.

8. The high-precision laser positioning unpacking path planning system according to claim 7, characterized in that, The motion control module specifically includes: A local trajectory generator is used to sample multiple short-term trajectories in the velocity space based on the dynamic window method in each control cycle, and select the optimal trajectory that takes into account path tracking, obstacle avoidance and speed through an evaluation function. The model predictive tracking controller is used to establish an optimization problem in the finite time domain with the goal of tracking the optimal trajectory. It solves the problem online under the premise of satisfying kinematic and dynamic constraints and outputs the linear velocity and angular velocity control commands of the robot. A multi-level anomaly monitor is used to monitor location confidence, path tracking error and local planning feasibility in real time, and trigger hierarchical recovery strategies such as deceleration, emergency stop or global replanning accordingly.

9. A high-precision laser positioning unpacking path planning system according to claim 8, characterized in that, The job execution module specifically includes: The task scene recognition and mode switching unit is used to identify the target by recognizing visual signs and switch the system control mode from path tracking to precise docking when the robot enters the preset range of the task point. A visual servo controller is used to control the visual sensors on a robot to identify feature points or planes on a target package, calculate feature space errors, and generate fine motion instructions that minimize the errors, guiding the robot to complete millimeter-level alignment. The task sequence triggering unit is used to send an orderly package of motion instructions to the robotic arm control system according to a preset process after the chassis is accurately docked and locked, and to monitor the execution status of the task.

Images