A high-precision laser positioning unpacking path planning system

Abstract

The application 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, a high-precision positioning module, a semantic map construction module, a global path planning module and a motion control module. The data synchronization module is used for multi-sensor hardware synchronization and data alignment; the high-precision positioning module is based on environmental skeleton features and a sliding window to optimize output accurate pose; the semantic map construction module fuses visual and laser data to generate a dynamic semantic grid map; the global path planning module utilizes a hybrid potential field to improve an A* algorithm to plan a smooth path in a skeleton channel; the motion control module realizes closed-loop motion control and safety monitoring through model predictive tracking; and the work execution module adopts visual servoing to complete millimeter-level accurate parking at a work point and trigger unpacking work. The application improves the positioning robustness under dynamic occlusion through environmental skeleton features, ensures the safety and efficiency of the path through semantic understanding and intelligent planning, and realizes full-process automation from navigation to accurate work.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot automatic control and industrial automation, and in particular to a high-precision laser positioning unpacking path planning system. BACKGROUND

[0002] With the development of intelligent manufacturing and smart logistics, automated unpacking operation has become a key link to improve warehouse turnover efficiency and reduce labor cost and labor intensity. At present, this link mainly relies on manual operation or automatic guided vehicle (AGV) based on fixed route. The former has low efficiency and poor consistency, and the latter cannot adapt to the complex requirements such as dynamic change of parcel stacking, limited operation space and man-machine mixing in unpacking scene. In order to realize flexible and intelligent automated unpacking, mobile robots need to have the ability of autonomous navigation and accurate operation in unstructured environment, which puts high requirements on its environment perception, positioning, planning and control technology.

[0003] The prior art mainly has the following deficiencies: firstly, in the aspect of perception and positioning, the traditional laser positioning method is prone to positioning drift or failure in the scene where parcels are dense and environmental features are temporarily blocked; only using single geometric information lacks semantic understanding of the operation object (such as parcels and shelves), and the robot cannot "recognize" the environment. Secondly, in the aspect of environment modeling, the representation method based on two-dimensional occupancy grid or cost map cannot distinguish different categories of objects and their states (such as fixed shelves and movable parcel stacks), resulting in that the path planning cannot make decisions consistent with the operation logic. Thirdly, in the aspect of path planning, the traditional global planning algorithm (such as A* algorithm) often only pursues the shortest path, which may make the robot travel close to the obstacle, and the safety redundancy is insufficient; and in dynamic environment, the active and predictive avoidance ability of dynamic obstacles (such as personnel and other AGVs) is lacking, and the planning efficiency and safety cannot be considered.

[0004] In view of the above problems, a high-precision laser positioning unpacking path planning system is needed to solve the above problems existing in the traditional method. SUMMARY

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

[0006] To achieve the above purpose, the technical solution adopted by the present application is as follows:

[0007] A high-precision laser positioning unpacking path planning system comprises:

[0008] sequentially connected data synchronization module, high-precision positioning module, semantic map construction module, global path planning module, motion control module and work execution module;

[0009] The data synchronization module is used for hardware time synchronization and spatial coordinate system unification of raw data of a laser radar, a visual sensor and an inertial measurement unit, and outputs a multi-modal data frame aligned in time and space;

[0010] The high-precision positioning module is used for receiving the multi-modal data frame, extracting environmental skeleton features and performing sliding window tight coupling optimization, and outputting a high-precision global pose of the robot;

[0011] The semantic map construction module is used for fusing the high-precision global pose, visual semantic information and laser point cloud, constructing and real-time updating a semantic occupancy grid map containing a static semantic layer and a dynamic obstacle layer;

[0012] The global path planning module is used for extracting an environmental safety skeleton, generating a semantic skeleton channel, constructing a fusion potential field based on the semantic occupancy grid map, and planning a smooth global path in the channel by using an improved A* algorithm;

[0013] The motion control module is used for generating driving instructions by local trajectory planning and model predictive tracking control according to the global path and real-time environmental information, and implementing closed-loop safety monitoring;

[0014] The work execution module is used for switching to a visual servo control mode to realize accurate parking and triggering a robotic arm to execute an unpacking work task when reaching a work point.

[0015] Further, the data synchronization module specifically comprises:

[0016] A hardware triggering unit is used for sending a unified pulse to the laser radar and the visual sensor to make them start collecting synchronously;

[0017] A time alignment unit is used for giving the synchronously collected laser frame and image frame the same time stamp, and performing interpolation alignment on IMU data;

[0018] A spatial calibration unit is used for applying a pre-calibrated extrinsic parameter matrix to coordinate correlate laser point cloud in the laser frame with image pixels in the image frame;

[0019] A distortion correction unit is used for performing motion distortion correction on the laser point cloud by using IMU data or odometer information, and outputting the multi-modal data frame aligned in time and space.

[0020] Further, the high-precision positioning module specifically comprises:

[0021] a geometry feature extraction unit configured to extract edge point features and plane point features from a laser point cloud of a current frame;

[0022] an environmental skeleton feature extraction unit configured to identify and extract long line segments and large planes from historical local maps as stable environmental skeleton features;

[0023] a two-stage scan matching unit configured to firstly perform inter-frame matching to estimate a preliminary motion, and then perform key frame-to-map matching based on the environmental skeleton features to obtain an initial pose;

[0024] a sliding window optimization unit configured 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 key frames, and output an optimized high-precision pose and a confidence.

[0025] Further, the semantic map construction module specifically comprises:

[0026] a visual semantic segmentation unit configured to perform pixel-level segmentation on image frames using a convolutional neural network, identify object-related categories of the unpacking path, and obtain a semantic segmentation result;

[0027] a point cloud semantic labeling unit configured to project the semantic segmentation result to a laser point cloud through coordinate transformation, assign semantic labels to each three-dimensional point, and generate a semantic point cloud;

[0028] a multi-layer Bayesian update unit configured to project the semantic point cloud to a two-dimensional grid map according to the robot pose, layer different semantic categories, and recursively update the occupancy probability of each layer grid in a logarithmic probability form;

[0029] a dynamic obstacle tracking unit configured to cluster and multi-target track obstacle point clouds labeled as dynamic, estimate their speed and predict short-term trajectories.

[0030] Further, the semantic map construction module further comprises a map decay unit configured 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 temporary obstacle information that has been removed.

[0031] Further, the global path planning module specifically comprises:

[0032] a passability determination and inflation unit configured to dynamically determine the passability of each semantic grid according to the current unpacking task, and inflate all impassable areas based on the robot's outer contour and safety margin to generate a binary safety map;

[0033] a topology skeleton extraction unit configured to apply a thinning algorithm to the passable region in the binary safe map to extract a one-pixel-wide environmental topology skeleton;

[0034] a semantic skeleton corridor generation unit configured to search for a shortest path connecting the start point and the target point on the topology skeleton, and to form a skeleton corridor for limiting a search range by adaptively expanding a width of the path according to distances of points of the path to obstacles;

[0035] a fusion potential field construction unit configured to calculate distances of points in the skeleton corridor to nearest obstacles and distances of the points to the skeleton to generate a static skeleton potential field, and to form a comprehensive potential field by superimposing a time-varying threat potential field from a predicted trajectory of a dynamic obstacle.

[0036] Further, the global path planning module further includes a path searching and smoothing unit configured to:

[0037] introduce a value of the comprehensive potential field as a cost item into a cost function of an A* algorithm to perform path searching in the skeleton corridor;

[0038] extract key points from an initial path obtained by the searching, and remove redundant nodes;

[0039] smoothly interpolate path segments between adjacent key points using a third-order Bezier curve to generate a final smoothed global path.

[0040] Further, the motion control module specifically includes:

[0041] a local trajectory generator configured to sample a plurality of short-term trajectories in a velocity space based on a dynamic window method in each control period, and to select an optimal trajectory giving consideration to path tracking, obstacle avoidance and velocity by using an evaluation function;

[0042] a model predictive tracking controller configured to establish an optimization problem in a finite time domain with a tracking optimal trajectory as a target, to solve the optimization problem online under the premise of satisfying kinematic and dynamic constraints, and to output linear velocity and angular velocity control instructions of the robot;

[0043] a multi-level abnormality monitor configured to monitor position confidence, path tracking error and local planning feasibility in real time, and to trigger a hierarchical recovery strategy such as deceleration, emergency stop or global re-planning accordingly.

[0044] Further, the work execution module specifically includes:

[0045] a work scene recognition and mode switching unit configured to confirm a target by recognizing a visual identifier when the robot enters a preset range of a work point, and to switch a system control mode from path tracking to precise parking;

[0046] 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.

[0047] 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.

[0048] In summary, the present invention has at least one of the following beneficial technical effects:

[0049] 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.

[0050] 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.

[0051] 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.

[0052] 3. Ensure the smoothness, real-time and safety of robot motion: the motion control module adopts a hierarchical control architecture, the upper local trajectory planner combines real-time environmental information for online obstacle avoidance and speed planning, and the lower model predictive tracking controller ensures that the robot accurately and smoothly tracks the trajectory. Combined with multi-level abnormal monitoring and recovery strategy, the system can respond to abnormal conditions such as positioning failure and path blockage from deceleration, emergency stop to re-planning, forming a closed-loop safety barrier from perception to control, greatly improving the robustness of the system in real dynamic environment.

[0053] 4. Ensure the accuracy and automation of the unpacking terminal: the work execution module designs a mode switching process from "global navigation" to "accurate docking". Through closed-loop control based on visual servo, the visual features of the target package are directly used for millimeter-level fine adjustment of the pose, solving the docking problem caused by the lack of absolute positioning accuracy in traditional methods. After docking is completed, the system can automatically trigger the preset robot work sequence, realizing seamless connection from navigation to work and full-process automation, significantly improving the accuracy and reliability of the work. BRIEF DESCRIPTION OF DRAWINGS

[0054] Figure 1 The figure is a schematic diagram of the system structure of the application. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical scheme and advantages of the application clearer, the application will be further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the application and do not limit the application. In addition, the technical features involved in each embodiment of the application described below can be combined with each other as long as they do not conflict with each other.

[0056] As shown in Figure 1 The application provides a high-precision laser positioning unpacking path planning system, which comprises:

[0057] sequentially connected data synchronization module, high-precision positioning module, semantic map construction module, global path planning module, motion control module and work execution module;

[0058] The data synchronization module is used for hardware time synchronization and spatial coordinate system of the raw data of the laser radar, visual sensor and inertial measurement unit, and outputs the spatio-temporal aligned multi-modal data frame;

[0059] The high-precision positioning module is used for receiving the multi-modal data frame, extracting the environmental skeleton features and performing sliding window tight coupling optimization, and outputting the high-precision global pose of the robot;

[0060] The semantic map construction module is configured to fuse high-precision global pose, visual semantic information and laser point cloud, and construct and update a semantic occupancy grid map including a static semantic layer and a dynamic obstacle layer in real time.

[0061] The global path planning module is configured to extract an environment safety skeleton, generate a semantic skeleton channel, construct a fusion potential field based on the semantic occupancy grid map, and plan a smooth global path in the channel by using an improved A* algorithm.

[0062] The motion control module is configured to generate driving instructions by local trajectory planning and model predictive tracking control according to the global path and real-time environment information, and implement closed-loop safety monitoring.

[0063] The work execution module is configured to switch to a visual servo control mode to achieve accurate parking and trigger the robotic arm to perform unpacking work when reaching the vicinity of the work point.

[0064] The above modules are introduced in detail as follows:

[0065] I. Data synchronization module

[0066] 1. Input:

[0067] Raw three-dimensional point cloud sequences from a laser radar, each point containing three-dimensional coordinates and reflection intensity information;

[0068] Image sequences from a visual sensor (such as an RGB-D camera), including high-resolution RGB color images and corresponding depth images;

[0069] Six-axis inertial data sequences from an inertial measurement unit (IMU), including three-axis acceleration and three-axis angular velocity.

[0070] 2. Output:

[0071] Multi-modal data frames that are strictly synchronized in time and have a unified coordinate system in space.

[0072] 3. Specific processing process:

[0073] (1) Hardware time synchronization trigger based on a hardware trigger unit

[0074] The robot main control computer sends a unified hardware trigger pulse signal to the laser radar and the RGB-D camera. When this pulse signal arrives, the laser radar immediately starts a frame of scanning, and the RGB-D camera immediately starts a frame of image exposure, ensuring that the two sensors physically start collecting a frame of data at the same time. This fundamentally eliminates system errors caused by different starting times of data collection. IMU data is independently and continuously output at its inherent high frequency;

[0075] (2) Software timestamp alignment and interpolation based on time alignment unit

[0076] The same trigger time timestamp (T0) is given to each frame of laser point cloud and RGB-D image collected by hardware trigger. For high-frequency IMU raw data output between two consecutive synchronous time T0 and T1, according to its own accurate microsecond timestamp, a linear interpolation method is used to normalize it to a discrete time sequence with T0 as the starting point and a fixed time interval (such as 10 milliseconds), forming an IMU discrete data stream aligned with the time reference of laser and vision data.

[0077] (3) Space coordinate system calibration based on space calibration unit

[0078] Through the hand-eye calibration process, the rigid transformation relationship between the laser radar coordinate system (Lidar Frame, {L}) and the vision sensor coordinate system (Camera Frame, {C}) is accurately determined, that is, 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, and these transformation parameters are embedded into the system as known constants.

[0079] (4) Point cloud distortion correction based on distortion correction unit

[0080] Because the laser radar needs time (usually tens of milliseconds) for one frame scanning, the points in the frame are collected at different instantaneous poses. The IMU pre-integrated data from T0 or the robot speed estimation of the last period is used to correct the motion distortion of the current frame laser point cloud, and all point cloud coordinates are converted to the {L} coordinate system at the same time T0.

[0081] Using the transformation relationship (R, t) obtained by calibration, the corrected laser point cloud in the {L} coordinate system at T0 is projected to the depth image plane and the RGB image plane in the {C} coordinate system at T0 through perspective projection transformation. For each laser point successfully projected into the image pixel, the association between its three-dimensional coordinates and the corresponding pixel's RGB color value and depth value is established. The spatial binding of vision texture information and laser geometric information is completed.

[0082] The T0 timestamp, motion distortion corrected laser point cloud, RGB image, depth image, and IMU pre-integrated data from the last synchronous time to T0 are packaged into a complete data frame, i.e. a multi-modal data frame, which ensures that all information reflects the state of the robot and the environment at T0.

[0083] Two, high-precision positioning module

[0084] 1. Feature extraction based on geometric feature extraction unit and environmental skeleton feature extraction unit.

[0085] (1) Input:

[0086] Current frame laser point cloud P from the data synchronization module current ;

[0087] Local sliding window map M maintained by historical data local The map is composed of stable feature points from several past keyframes.

[0088] (2) Output:

[0089] The set of local geometric features F extracted from the point cloud of the current frame current This includes edge points and planar points;

[0090] 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.

[0091] (3) Specific process:

[0092] 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.

[0093] 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.

[0094] Edge points: Points with large curvature, usually originating from the edges of objects, corners, etc.

[0095] Planar point: A point with small curvature, usually originating from large flat surfaces such as walls, floors, and tabletops;

[0096] General points: Points where the curvature is centered, not used for fine registration;

[0097] 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.

[0098] Environmental skeleton feature extraction: This is a method in M local The process of combining offline and online methods in the following context:

[0099] For M localthe stable planar point set in F

[0100] extract the boundary of these large planar surfaces and fit a line segment to it, obtaining the contour line of the planar surface (e.g. the upper and lower boundary lines of wall W1).

[0101] define the above large planar surface and its long boundary line as F skeleton These features are inherent to the environment, stable in the long term, and provide global constraints for localization, which are not sensitive to the occlusion of temporarily piled up packages.

[0102] 2. Two-stage scan matching and initial pose estimation based on two-stage scan matching unit

[0103] (1) Input:

[0104] F current (current frame feature), F skeleton (environmental skeleton feature);

[0105] the optimal pose estimation of the robot at the last time T prev ;

[0106] (2) Output:

[0107] the initial pose estimation of the current frame T init ;

[0108] (3) Specific process:

[0109] Inter-frame matching based on motion model: based on T prev , combined with the short-term relative motion prediction provided by the robot wheel odometer or IMU, a pose prediction T predict is obtained. Fast matching (such as using a variant of ICP) is performed between F current and the features of the last frame to fine-tune T predict , obtaining a high-frequency but possibly drifting inter-frame pose T odom , which is used to compensate for rapid motion.

[0110] Global matching based on skeleton features: take T odom as the initial guess, perform key frame to skeleton map matching, specifically:

[0111] associate the planar points in F current with the large planar surfaces in F skeleton (compute the distance from the point to the plane);

[0112] associate the edge points in F current with the long boundary lines in F skeleton (compute the distance from the point to the line);

[0113] Construct and minimize the sum of these distance errors, the nonlinear optimization solves the optimal pose T that makes the current frame features fit to the environment skeleton init , which effectively corrects the cumulative drift of inter-frame matching with globally stable features.

[0114] 3. Sliding window tight coupling optimization based on sliding window optimization unit

[0115] (1) input

[0116] All poses [T k , T k-1 ,..., T init ] of a sliding window (such as the last 10 key frames) including the current frame;

[0117] The feature point cloud corresponding to each frame in the sliding window;

[0118] Pre-integration constraints from IMU;

[0119] (2) output

[0120] Optimized poses of all key frames in the sliding window after global optimization, especially the optimized pose T optimized of the current frame;

[0121] (3) specific process

[0122] Construct a factor graph: model the optimization problem as a factor graph:

[0123] Nodes: variables to be optimized, i.e. the poses of key frames in the sliding window.

[0124] Edges (constraint factors):

[0125] Laser odometry factor: relative pose constraints between adjacent key frames generated by inter-frame matching;

[0126] Skeleton feature factor: constraints between the pose of each key frame and the skeleton features (planes, lines) observed by it, which is the key to tight coupling, and it directly injects global skeleton information into the optimization framework;

[0127] IMU pre-integration factor: relative motion constraints between adjacent key frames obtained by integrating IMU measurements, providing high-frequency attitude change information;

[0128] Loop closure factor: if the current frame is successfully matched with a historical non-adjacent frame, add a strong constraint to correct the drift.

[0129] Non-linear optimization solver: using Levenberg-Marquardt or other optimization algorithms, adjust the values of all pose nodes so that the total error of all factor constraints on the graph is minimized.

[0130] Marginalization and window update: after optimization, marginalize the oldest frame in the window and its constraints, keep its information as prior, while add a new frame, maintain the window size constant, realize continuous and recursive optimization.

[0131] 4. Finally, based on the positioning quality evaluation and output unit to evaluate the positioning quality and output

[0132] (1) input

[0133] Optimized pose T optimized ;

[0134] Residual, covariance information calculated during optimization;

[0135] (2) output

[0136] Final pose T final ;

[0137] The confidence score C of this positioning (for example, the inlier rate based on skeleton feature matching, the size of the residual after optimization, etc.);

[0138] (3) specific process:

[0139] Confidence calculation: analyze the optimization results, for example, if the current frame and the skeleton feature matching are successful, the number of inliers is large and the distribution is uniform, and the residual after optimization is small, then the confidence C is high; If the matching skeleton feature is less due to serious occlusion, C is reduced.

[0140] Decision and output:

[0141] If C is higher than the threshold, directly output T optimized as T final ;

[0142] If C is lower than the threshold, it is determined that the positioning may not be reliable, and the system can take two strategies:

[0143] a) Trigger a warning and try to combine the wheeled odometer for short-time dead reckoning;

[0144] b) Based on T optimized and low confidence information, inform the subsequent path planning module to take a more conservative strategy (such as slowing down, searching for a safer path).

[0145] Feature map update: according to T final , the stable feature points in the current frame that are not associated with dynamic objects are supplemented and updated to the local sliding window map M localIn China, the representation of the environment continues to be enriched.

[0146] III. Semantic Map Construction Module

[0147] 1. Point cloud semantic annotation based on visual semantic segmentation units and point cloud semantic annotation units

[0148] (1) Input:

[0149] High-confidence optimized pose T of the current frame from the high-precision positioning module final ;

[0150] RGB image I from the data synchronization module, synchronized with the current frame time rgb and the corresponding depth image I depth ;

[0151] The current frame laser point cloud P, already aligned with the image, from the data synchronization module. calibrated ;

[0152] 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".

[0153] (2) Output:

[0154] 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.

[0155] (3) Specific process:

[0156] 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.

[0157] 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. labelIn the pixel coordinate system, for the points successfully projected into the image range, directly assign the semantic label of the pixel where the laser point is projected to the laser point. For the points projected outside the image range (such as the part of the laser radar field of view wider than the camera), temporarily mark their labels as "unlabeled".

[0158] Depth-based label verification and repair: To improve reliability, consistency check is performed:

[0159] Calculate the three-dimensional coordinates of the laser points and the corresponding pixel depth values I depth The distance between the inverse calculated three-dimensional coordinates. If the distance is too large (exceeding a certain threshold, such as 5 cm), it is considered that the projection matching is unreliable, which may be caused by dynamic object movement or minor registration errors. Temporarily mark the label of this point as "pending";

[0160] For "unlabeled" and "pending" points, use a method based on point cloud neighborhood statistics to repair the labels: check the nearest K neighborhood points that have been successfully labeled. If a certain semantic label accounts for more than a certain percentage (such as 70%), assign this label to the current point. Otherwise, mark it as "unknown obstacle".

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

[0162] 2. Construct and update the semantic occupancy grid map based on multi-layer Bayesian update unit and map decay unit

[0163] (1) Input

[0164] Current frame semantic point cloud P semantic ;

[0165] Current frame pose T final ;

[0166] A global, to-be-updated two-dimensional multi-layer semantic occupancy grid map M semantic_grid Each layer of the map corresponds to a semantic class (such as "ground layer", "shelf layer", "package layer", "dynamic obstacle layer"). Each grid stores two core states: log odds occupancy probability logarithm and age last observation time frame.

[0167] (2) Output

[0168] Updated global semantic occupancy grid map M semantic_grid .

[0169] (3) Detailed process:

[0170] Point cloud gridding and projection: use Tfinal All points in the current frame semantic point cloud P semantic are converted from sensor coordinate system to global world coordinate system. Then, these three-dimensional points are projected vertically to a two-dimensional horizontal plane (ground plane), and their global two-dimensional coordinates (X, Y) are determined to determine their corresponding grid index (i, j).

[0171] Bayesian probability update: for each hit grid, the probability is updated on its corresponding semantic class layer. The log probability form of the binary Bayesian filter is used for recursive update:

[0172] For the layer consistent with the point cloud semantic label L: the log odds value of the grid is increased by a positive observation value log(p / (1-p)), where p is the probability of hitting observation (such as 0.7);

[0173] For all other semantic layers: since the position is observed to be occupied by class L, counter-evidence is formed for other classes, and the log odds value of the corresponding grid of these layers is increased by a negative observation value (i.e. decreased by a fixed value);

[0174] At the same time, the age of the grid in all layers is updated to the current time frame.

[0175] Empty area update and dynamic decay: for the grid within the current sensor field of view (FOV) but not hit by any point cloud, "empty" update is performed:

[0176] The log odds value of all layers is decreased by a fixed value (indicating that the position is empty evidence enhancement);

[0177] Dynamic decay mechanism is introduced: traverse the map, for the grid whose age exceeds a certain number of frames (such as N=50 frames) and is not updated, the log odds value will slowly decay to the initial value over time. This makes the map gradually "forget" those objects that have not appeared for a long time (may be removed), especially the information in the "package layer" and "dynamic obstacle layer".

[0178] Map state extraction: at any time, the final state can be analyzed according to the log odds value of each grid in all layers:

[0179] Static occupancy: if the log odds of the static layer such as "shelf layer" or "ground layer" is continuously higher than the threshold value, it is considered that the grid is occupied by a static object;

[0180] Semantic occupancy: if the log oddsIf the highest and exceeds the threshold, it is considered that the grid is occupied by the package, and its height characteristics (from the original three-dimensional point cloud statistics) are recorded;

[0181] Free space: if the log odds of all layers is lower than the low threshold, it is considered that the grid is free;

[0182] Dynamic / temporary occupation: if the log odds value of the "dynamic obstacle layer" or "package layer" is high but the age is new, and the static layer value is low, it is marked as temporary occupation.

[0183] 3. Dynamic obstacle layer maintenance and prediction based on dynamic obstacle tracking unit

[0184] (1) Input

[0185] Dynamic point set in the current frame semantic point cloud P semantic with labels "pedestrian" and "AGV";

[0186] The pose of the current frame and the previous several frames;

[0187] Dynamic obstacle layer in the semantic occupancy grid map M semantic_grid at the current time;

[0188] (2) Output

[0189] Dynamic obstacle list D list , including the estimated position, velocity, bounding box and predicted short-term trajectory of each dynamic obstacle.

[0190] (3) Specific process:

[0191] 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. A multi-target tracking algorithm (such as Kalman filter or a simpler IOU tracker) is used to associate the point clusters detected in the current frame with the dynamic obstacle trajectories maintained in the last frame.

[0192] State estimation and map update: for successfully associated trajectories, the state (center position, velocity) is updated using Kalman filtering. The current position and estimated occupancy area (according to the point cluster bounding box) are updated to the "dynamic obstacle layer" of M semantic_grid , and the log odds value of the region is set to a high value and the age is the latest.

[0193] Short-term trajectory prediction: based on the current estimated velocity and position, the motion trajectory of each active dynamic obstacle in the future short time (such as 1-3 seconds) is predicted. The predicted trajectory is saved in D listCenter.

[0194] Abnormal handling: for unmatched historical trajectories, continue to predict 1-2 frames. If multiple frames are not matched in succession, remove it from the active list, and start the decay process of the region in the map "dynamic obstacle layer".

[0195] Four, global path planning module

[0196] 1. Based on the passability determination and inflation unit and topological skeleton extraction unit, the semantic map preprocessing and skeleton extraction are carried out

[0197] (1) input:

[0198] The latest semantic occupancy grid map M from the semantic map construction module semantic_grid , containing multi-layer semantic information and dynamic obstacle position;

[0199] The physical contour radius R of the robot robot And the preset additional safety margin δ;

[0200] The current unpacking task description, including the target work point coordinates and the work type (such as "whole pile picking" and "single piece picking").

[0201] (2) output:

[0202] Binary safety inflation map M for path search binary (The passable area is 1 and the impassable area is 0);

[0203] The environment topological skeleton map M extracted from M binary skeleton , composed of a series of single-pixel width center lines.

[0204] (3) specific process:

[0205] Semantic information fusion and passability determination: according to the work task, dynamically generate the "passability determination rule" of the current frame. The present invention provides an embodiment, specifically:

[0206] Static layer: fixed shelves, walls are always impassable;

[0207] Dynamic layer: the current position of dynamic obstacles (personnel, other AGVs) is impassable;

[0208] Work semantic layer: the passability of the parcel pile area is dynamically determined according to the task: if it is "whole pile picking", the area near the target pile is considered impassable before work (to prevent collision), and is considered passable after work; The passage area is always a high-priority passable area.

[0209] ​Safety distance inflation: To ensure the robot body does not collide with any obstacles, for all the grids determined as impassable (including static obstacles and dynamic obstacles), take them as the center, and perform a morphological inflation operation (e.g. disk inflation) with radius R inflate =R robot +δ. The inflated region is marked as impassable (0), and the rest is marked as passable (1), generating a binary map M binary . The robot size is converted into a point robot, and the path planning problem is solved.

[0210] Environment skeleton extraction: Apply a thinning algorithm to the passable regions (regions with value 1) in M binary . The algorithm iteratively removes the boundary points of the passable regions until all regions are reduced to single-pixel-wide centerlines, while maintaining the topological connectivity of the original regions. This process generates an environment skeleton M skeleton , which represents the "hub" of all safe passages in the environment, maximally away from the boundaries of the inflated obstacles on both sides, naturally having high safety.

[0211] Skeleton optimization: Post-process the extracted initial skeleton, including: removing isolated branches (spurs) that are too short in length; smoothing the jagged skeleton while ensuring connectivity; storing key intersection points and endpoints as topological nodes.

[0212] 2. Skeleton passage generation based on semantic skeleton passage generation unit

[0213] (1) Input:

[0214] Skeleton map M skeleton ;

[0215] Binary safety map M binary ;

[0216] The current pose of the robot P current and the target point P goal ;

[0217] The semantic context at the target point, such as: the target is "package pile A".

[0218] (2) Output:

[0219] The restricted skeleton passage region Channel connecting the start and end points, represented as a set of grids;

[0220] The basic passage cost C base (i, j) corresponding to each grid in the skeleton passage;

[0221] (3) Specific process:

[0222] Start / end point and skeleton docking: In Mskeleton Find the distance P current from P goal and P start to S goal . Make sure the line segment connecting P current to S start and P goal to S goal is fully drivable in M binary .

[0223] Initial skeleton path search: On the skeleton map M skeleton , search for the shortest path from S start to S goal using Dijkstra algorithm (in terms of skeleton point number). This path is a polyline L skeleton composed of a series of skeleton points, which is the centerline of the global safe passage.

[0224] Generate dynamic skeleton passage: Based on the skeleton path L skeleton , generate a passage area with a dynamic width. For each point on L skeleton , calculate its distance d safe to the nearest inflated obstacle boundary. The width of the passage at this point is defined as W = 2 * β * d safe , where β is the passage coefficient, e.g. 0.8, ensuring that the passage is fully within the safe area. Take the union of the width areas of all points to get the final skeleton passage Channel, which greatly reduces the search space from the full map to a strip area around the safe centerline, greatly improving search efficiency.

[0225] Assign semantic basic cost: Traverse each grid in the passage Channel and assign it a basic passage cost C base based on the original semantic information (before inflation). Guide the planner to prefer routes with higher operational efficiency under the premise of safety.

[0226] 3. Construct the cost function of the fusion skeleton potential field and the dynamic threat field based on the fusion potential field construction unit

[0227] (1) Input:

[0228] Skeleton passage Channel;

[0229] Skeleton map M skeleton ;

[0230] Binary safe map M binary (containing dynamic obstacle inflation area);

[0231] Dynamic obstacle prediction trajectory list D list ;

[0232] (2) Output:

[0233] The combined potential value P of each grid cell within the skeleton channel total (i, j);

[0234] The improved A* algorithm cost function is F(n) = g(n) + h(n) + λ*P. total (n);

[0235] (3) Detailed process:

[0236] 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.

[0237] 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.

[0238] 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].

[0239] Define the improved cost function: The total cost F(n) of the A* algorithm consists of three parts:

[0240] g(n): The actual cost from the starting point to node n, accumulated C base (Semantic cost) and travel distance;

[0241] h(n): heuristic cost from node n to goal (e.g. Euclidean distance);

[0242] λ*P total (n): potential field penalty term, λ is the potential field weight coefficient, when P total (n) exceeds the threshold of corresponding safety distance d safe , it can be set to infinity to prohibit search.

[0243] 4. Intra-lane A* search and path post-processing based on path search and smoothing unit

[0244] (1) Input:

[0245] skeleton lane Channel;

[0246] improved cost function F(n);

[0247] start point P current and target point P goal .

[0248] (2) Output:

[0249] an initial optimized path Path raw composed of a series of path points;

[0250] (3) Specific process:

[0251] restricted A* search: run the standard A* algorithm within the grid set defined by the skeleton lane Channel, and the open set and closed set of the algorithm only contain the grids within the lane. Each time the node is expanded, the improved cost function F(n) is used for evaluation. This ensures that the path searched is not only short in length, but also automatically tends to walk in the center of the lane and avoids dynamic threats.

[0252] key point extraction: remove the collinear redundant nodes in Path raw , and only keep the key turning points where the direction of the path changes significantly.

[0253] third-order Bezier curve smoothing: to meet the robot motion continuity requirement, use a third-order Bezier curve to smooth the path segment between adjacent key points. Finally, a smooth, derivable, and robot-executable global path Path global is generated.

[0254] Five, motion control module

[0255] 1. Local trajectory generation and speed planning based on local trajectory generator

[0256] (1) Input:

[0257] Global path from the global path planning module global ;

[0258] Kinematic model of a robot;

[0259] 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 ];

[0260] Real-time dynamic obstacle list D list And its predicted trajectory.

[0261] (2) Output:

[0262] A timestamped local trajectory that satisfies motion constraints (Trajectory) local .

[0263] (3) Detailed process:

[0264] 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.

[0265] 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:

[0266] The trajectory of the simulated robot after moving at this speed for a short time interval Δt;

[0267] Evaluation function calculation: Calculates the evaluation score for the simulated trajectory, which is composed of multiple weighted components:

[0268] Goal orientation: The degree of proximity between the trajectory endpoint and the local target point (the point at Lookahead_distance on the path);

[0269] Path fit: Trajectory and global reference path global The average deviation;

[0270] Speed preference: encourage to go faster;

[0271] Dynamic obstacle avoidance: check if the trajectory collides with the predicted trajectory of dynamic obstacles in the future time horizon. list If collision happens, the score is negative infinity, which directly rejects the speed pair.

[0272] Static safety: check if the trajectory collides with static obstacles in the real-time semantic map.

[0273] Optimal speed selection and trajectory generation: from all the un-rejected speed pairs, select the one with the highest evaluation score as the command for the next control period. opt , ω opt ) as the command for the next control period. opt , ω opt ) and the kinematic model of the robot, integrate to generate a local predicted trajectory Trajectory local for the next N control periods as the reference input for the controller.

[0274] 2. Model predictive tracking control based on the model predictive tracking controller

[0275] (1) Input:

[0276] Local reference trajectory Trajectory local ;

[0277] High-precision real-time pose feedback Pose feedback and its confidence C;

[0278] Kinematic model and constraints of the robot.

[0279] (2) Output:

[0280] Final control command sent to the robot drive motor: left wheel target speed v l_cmd , right wheel target speed v r_cmd ;

[0281] (3) Specific process:

[0282] Construct optimization problem: at each control period, take the current pose Pose feedback as the initial state, and establish a model predictive control problem in a finite time horizon (e.g. 2 seconds). The optimization variable is a series of control quantities (v, ω) in the future time horizon.

[0283] Design objective function: the objective function aims to minimize:

[0284] Tracking error: the deviation of the predicted trajectory from the local reference trajectory Trajectory local in pose.

[0285] Control variation: sharp change of velocity command from the previous control period to ensure smooth motion;

[0286] Terminal error: deviation of the predicted end-of-horizon state from the reference trajectory end-of-horizon state.

[0287] Apply constraints: impose hard constraints in the optimization problem to ensure that the solution satisfies:

[0288] Kinematic constraints: v min ≤ v ≤ v max , ω min ≤ ω ≤ ω max ;

[0289] Dynamic constraints: |v k - v {k-1} | / Δt ≤ a max , |ω k - ω {k-1} | / Δt ≤ α max ;

[0290] Obstacle avoidance constraints (optional hard constraints): all points on the predicted trajectory must lie within the drivable region of the semantic map.

[0291] Solve and output in real-time: solve the constrained optimization problem (usually converted to a quadratic program) to obtain the optimal control sequence. Take the first control quantity (v l_cmd , ω r_cmd ) of the sequence, back-calculate the left and right wheel speed commands (v track , v DWA ) according to the kinematic model, and send them to the driver.

[0292] 3. Multi-level anomaly monitoring and recovery strategy based on multi-level anomaly monitor

[0293] (1) Input:

[0294] Pose confidence C;

[0295] Controller tracking error e track ;

[0296] Flag indicating whether the local planner found a valid speed DWA ;

[0297] Sudden obstacle information in the real-time semantic map.

[0298] (2) Output:

[0299] System state identifier: NORMAL, CAUTION, ESTOP, or RECOVERY;

[0300] Recovery strategy instructions corresponding to the system state identifier.

[0301] (3) Specific process:

[0302] Status monitoring: Continuously monitor the following key metrics:

[0303] C <C th_low The location accuracy is too low, and the location may fail.

[0304] e track >e th_max The tracking error is too large, and the path is seriously deviated.

[0305] 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.

[0306] An unexpected static obstacle suddenly appeared ahead.

[0307] Tiered response strategy:

[0308] 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.

[0309] 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.

[0310] RECOVERY state: Upon triggering, the following process is executed:

[0311] 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.

[0312] 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.

[0313] c. Manual confirmation: If the autonomous recovery fails, an anomaly is reported and remote operator intervention is awaited.

[0314] 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.

[0315] VI. Job Execution Module

[0316] 1. Work area approximation and mode switching based on the work scene recognition and mode switching unit.

[0317] (1) Input:

[0318] global path end point P goal (job area entry point);

[0319] precise contour and feature point information of target parcel pile in semantic map;

[0320] current pose of robot.

[0321] (2) Output:

[0322] parking mode switching instruction;

[0323] vision-based fine local navigation target P fine .

[0324] (3) Specific process:

[0325] Region arrival determination: when the distance between the robot's center of mass and P goal is less than the threshold D approach (such as 1 meter), and the speed is lower than the threshold, it is determined that the job approach area has been entered.

[0326] Job scene recognition: through the vision sensor, identify the preset job scene identifier (such as a two-dimensional code, ArUco code) or directly identify the specific visual features of the target parcel pile to confirm the target accurately.

[0327] Mode switching: the navigation system switches from global path tracking mode to "job point precise parking mode. In this mode, the global path tracking controller is disabled, and the parking controller designed for low speed and high precision is enabled.

[0328] 2. Vision servo controller based vision servo precise positioning and parking

[0329] (1) Input:

[0330] desired features of the target parcel pile in the robot coordinate system, such as: three-dimensional coordinates P mark_desired of specific marker points on the parcel, or normal vector n desired of the parcel plane;

[0331] current features P mark_current or n current extracted by the vision sensor in real time;

[0332] laser positioning pose.

[0333] (2) Output:

[0334] vision servo control instructions (v vision , ω vision ) for fine alignment.

[0335] (3) Specific process:

[0336] Feature extraction and matching: Use RGB-D camera to detect visual features (such as SIFT, ORB feature points) or known geometric markers on the target package in real time, calculate the pixel position of the features in the current image through feature matching or template matching, and combine with depth information to solve its three-dimensional coordinates P in the robot camera coordinate system mark_current . At the same time, the point cloud fitting normal vector n of the main plane of the package can be calculated current .

[0337] Error calculation: Construct image / feature space error e. For example, for position 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.

[0338] Visual servo control law: Use image-based visual servoing or position-based visual servoing. The controller calculates the required robot end (or chassis) motion speed (v vision , ω vision ) according to the error e, so that the error tends to zero. This control law is directly integrated into the model predictive controller as the tracking target for the current period, i.e. the controller target is no longer a path point, but to minimize the feature error.

[0339] Iterative alignment: The robot moves slowly, and the visual servoing loop continues to work until the norm of the error e is less than a set millimeter level threshold (such as |e| < 5mm), and is maintained stable for a certain time (such as 1 second).

[0340] 3. State locking and work instruction triggering based on task sequence triggering unit

[0341] (1) Input:

[0342] Visual servoing error e and its stable state;

[0343] Current pose and speed of the robot;

[0344] Pre-set unpacking work process.

[0345] (2) Output:

[0346] Robot chassis locking instruction;

[0347] Work task instruction package sent to the robot arm control system.

[0348] (3) Specific process:

[0349] Docking confirmation and locking: After the fine alignment is completed and the robot is completely stationary, a docking completion signal is sent. The chassis controller switches to a position holding mode, attempting to maintain the current position even under minor disturbances.

[0350] Task sequence triggering: The central controller sends instruction packages to the robot arm control system in sequence according to the job type. For example:

[0351] Instruction 1: POWER_ON_AND_HOME (robot arm power on and home);

[0352] Instruction 2: MOVE_TO_PRE_GRASP_POSE (move to pre-grasp pose);

[0353] Instruction 3: EXECUTE_CUTTING_TRAJECTORY (execute cutting trajectory);

[0354] Instruction 4: GRASP_AND_LIFT (grasp and lift).

[0355] Job process monitoring: During the robot arm job, the robot chassis remains locked. Through robot arm state feedback and visual monitoring, it is ensured that the job proceeds according to the process. If the job fails (such as the grasp falling off), a retry process or an exception can be triggered.

[0356] Job completion and reset: After receiving the robot arm job completion signal, the chassis is unlocked, and a recovery state is triggered, usually planning a path from the current point back to the charging area or the next job point, the system switches back to the global path tracking mode, and a new task cycle begins.

[0357] Embodiments of the present application can be provided as a method, system, or computer program product. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage devices, etc.) containing computer usable program code.

[0358] The present application is described with reference to flowcharts and / or block diagrams of the method, device (system), and computer program product according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, as well as combinations of flows and / or blocks in the flowcharts 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, a special-purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing apparatus produce a device that implements the functions specified in the flowcharts and / or block diagrams. Figure 1one or more processes and / or blocks Figure 1 an apparatus for performing the functions specified in the flowchart

[0359] These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the flowchart Figure 1 one or more processes and / or blocks Figure 1 an apparatus for performing the functions specified in the flowchart

[0360] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the flowchart Figure 1 one or more processes and / or blocks Figure 1 an apparatus for performing the functions specified in the flowchart

[0361] The content of the present invention described in the specification is not detailed, which is the prior art known to those skilled in the art. It is pointed out that the above description is helpful for those skilled in the art to understand the present invention, but is not limited to the protection scope of the present invention. Any equivalent replacement, modification, improvement and deletion of the above description without departing from the essential content of the present invention falls within the protection scope of the present 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