A trajectory planning and optimization method for AGV flexible picking and placing scene

Abstract

The application belongs to the technical field of automatic guided vehicle motion planning, and discloses a trajectory planning and optimization method for AGV flexible picking and placing scene. The method is based on offline simulation to construct a planning parameter space lookup table indexed by tray lateral offset, longitudinal offset and deflection angle, and to determine the pre-adjustment driving strategy and the start and end points of the main trajectory online. A mixed A* search path initial solution is introduced, and curvature and smoothing pre-optimization is carried out. Then, a cubic B-spline fitting is performed, and the control points are optimized by a multi-objective penalty function containing a fitting maintenance penalty term, a curvature penalty term, a safety distance penalty term, a high-order smoothness term and an end straight driving penalty term. After curvature upper limit verification and safety distance verification, the trajectory meeting the vehicle kinematics constraints and the fork entry requirements is output.

Description

Technical Field

[0001] This invention belongs to the field of automated guided vehicle motion planning technology, specifically relating to a trajectory planning and optimization method for AGV flexible cargo retrieval and placement scenarios. Background Technology

[0002] Automated Guided Vehicles (AGVs) are widely used in warehousing and logistics, intelligent manufacturing, and automated material handling. For forklift AGVs, picking up and placing pallets is the core operation, and the accuracy of this operation directly determines the overall line cycle time and the integrity of the goods. Under ideal conditions, the pallet is located in a preset position in a standard storage location, and the AGV can complete forklift entry, lifting, and exit along a fixed taught straight trajectory. However, in actual production sites, pallets often exhibit multi-dimensional pose deviations, including lateral offset, longitudinal offset, and deflection angle, due to human placement errors, secondary handling by forklifts, ground friction displacement, cumulative deviations from multiple turnovers, or storage location calibration drift. When the deviation exceeds the tolerance capacity of the taught trajectory, the fixed trajectory scheme will directly fail, and an online trajectory planning method must be used to regenerate a trajectory that meets the non-integrity constraints and straight-line forklift entry requirements.

[0003] To address this issue, the representative AGV path planning methods in the existing technology mainly include the following four categories, but each of them has insurmountable limitations in the flexible picking and placing scenario of front-fork AGVs.

[0004] The first category comprises path planning methods based on topological graphs or road networks (such as the application of Dijkstra's and A* algorithms on fixed road networks). These methods abstract the workspace as a graph structure of nodes and edges, and the planned path can only travel within the pre-defined road network. Their fundamental limitation lies in the fact that the road network topology cannot represent the continuous pose space of the vehicle near the parking space's pre-positioning point. Therefore, when the pallet is offset or deflected, there are no discrete nodes in the road network that can accurately align with the pallet fork slots, and the planned path cannot meet the pose accuracy requirements for straight-line fork entry at the end of the path.

[0005] The second category comprises path planning methods based on grid maps (such as classic A*, D*, and their variants). These methods typically approximate the AGV as a point mass or an occupied grid with a fixed outline, and the searched path is formed by stitching together eight- or sixteen-neighbor distance segments. Their fundamental flaw lies in the lack of a vehicle kinematics model, resulting in frequent sharp angles exceeding the minimum turning radius capability in the search results. They also lack the ability to model the non-holonomic constraints of fork-type AGVs (i.e., the vehicle cannot slide laterally, the steering angle is limited, and the minimum turning radius is constrained by the wheelbase). Furthermore, these methods only control the path's geometric position and cannot constrain the end-point heading angle, making it difficult to ensure the AGV enters the pallet fork slot in the correct posture.

[0006] The third category comprises path planning methods based on random sampling (such as RRT, RRT*, InformedRRT*, etc.). These methods find feasible paths by randomly sampling in the state space and expanding the tree structure. Their limitations in flexible pick-up and drop-off scenarios are: firstly, the sampling process is random, with low success rates in narrow aisle areas near the storage location, resulting in unstable planning time and difficulty meeting production line cycle time requirements; secondly, the original RRT output path contains a large amount of redundant jitter, often requiring post-processing for smoothing, but the curvature distribution after smoothing cannot be guaranteed to be controllable; and thirdly, these methods lack cost differentiation for forward / reverse switching, easily outputting paths with multiple reversals.

[0007] The fourth category is motion planning methods based on HybridA*. This type of method has been widely adopted since the Stanford team's work on the DARPA Challenge. It extends motion primitives on the state grid and combines Reeds-Shepp or Dubins curves for analytical connections, enabling the output of continuous paths that satisfy non-holonomic constraints. It is currently the solution closest to the application scenario requirements of this invention. However, when directly applied to the flexible loading and unloading scenario of front-fork AGVs, the following specific technical shortcomings still exist.

[0008] Defect 1: Insufficient kinematic modeling. The first three methods mentioned above do not incorporate a non-holonomic kinematic model of the vehicle, failing to guarantee that the output path satisfies the minimum turning radius constraint and end-point heading constraint at every sampling point. This makes the path unexecutable when directly applied to front-fork AGVs. While Hybrid A* introduces kinematic extension during the search phase, it still lacks a systematic hard constraint verification mechanism for the upper limit of curvature in the subsequent smoothing and optimization stages, potentially leading to the optimized trajectory locally exceeding the vehicle's capabilities.

[0009] Defect 2: The analytical connection cost is insensitive to reversing behavior. In traditional hybrid A*, the cost function of the Reeds-Shepp curve consists only of geometric length (i.e., C_RS=L), without explicitly penalizing the number of direction changes between forward and reverse segments. In situations where the space at the storage location is limited, the shortest geometric length path often involves two or even three direction changes. For cargo AGVs, each direction change requires a complete time sequence of deceleration, stopping, shifting gears, and restarting, significantly slowing down the cycle time and causing slippage, misalignment, or even tipping of goods on the pallet due to the reversal of inertial force direction, which is particularly detrimental to fragile or high-center-of-gravity goods. This problem has not yet been solved directly and quantifiable at the cost level in existing publicly available solutions.

[0010] Defect 3: Inconsistent sampling spacing exists at the junction of the regular extended segment and the analytical connection segment in the search results. The hybrid A* regular extended segment is discretized with a fixed arc length Δs, while the Reeds-Shepp analytical segment is generated with independent sampling density. The spacing between adjacent path points at the junction of the two segments may differ by several times. If the discrete curvature gradient is directly calculated and optimized for such non-uniform sampling paths, abnormally amplified pseudo-gradients will be generated in short-spacing segments, causing local distortion of the optimized path at the junction. Existing smoothing methods mostly use simple interpolation or first-order low-pass filtering, and do not provide a dedicated gradient normalization mechanism for this non-uniform sampling problem.

[0011] Defect 4: Existing smoothing and optimization methods have a single constraint dimension, making it difficult to simultaneously meet the complex requirements of the pick-up and drop-off process. Actual engineering requires the final trajectory to simultaneously satisfy: curvature not exceeding the vehicle's capacity limit, maintaining a safety margin from obstacles, ensuring the end-point heading is strictly aligned with the forklift entry direction, end-point curvature approaching zero to ensure second-order continuity with the straight-line forklift entry segment, and maintaining high-order smoothness throughout the path to suppress abrupt acceleration changes. Existing solutions typically optimize only one or two of these (e.g., only curvature constraints, or only obstacle avoidance), lacking a unified multi-objective optimization framework and failing to provide dedicated constraint terms for the end-point straight-line forklift entry constraints unique to front-fork AGVs. When multiple requirements overlap, existing solutions often require multiple sequential processing steps with mutual sacrifices, making it difficult to guarantee the final trajectory quality.

[0012] Defect 5: Existing solutions lack a mechanism for predicting the feasibility of online planning. Current AGV systems generally adopt a reactive, error-handling approach, attempting planning first and reporting failures afterward. In scenarios with significant pallet deviation, the system may only discover an infeasible solution after performing a complete hybrid A* search and subsequent optimization (taking hundreds of milliseconds to seconds). Triggering manual intervention or rescheduling at this point will cause production line downtime and wasted computing power. Existing solutions do not systematically characterize the feasibility boundary of the pallet deviation parameter space offline; therefore, each online run requires repeated attempts to find infeasible solutions, resulting in significant efficiency losses.

[0013] In summary, existing AGV path planning technologies generally suffer from five specific technical problems when facing flexible picking and placing scenarios of front-fork AGVs: insufficient kinematic modeling (as mentioned in defect 1), lack of direction-changing cost (as mentioned in defect 2), optimization instability caused by non-uniform sampling (as mentioned in defect 3), lack of a unified framework for multiple constraints (as mentioned in defect 4), and lack of feasibility prediction (as mentioned in defect 5). Summary of the Invention

[0014] I. Technical problems to be solved

[0015] The technical problem to be solved by this invention is: in flexible loading and unloading scenarios where pallets have lateral offset, longitudinal offset and deflection angle deviation, how to quickly plan a trajectory for AGV that meets vehicle kinematic constraints, curvature upper limit constraints, safety distance constraints and end straight fork entry constraints, while reducing unnecessary switching between forward and reverse, and having the ability to predict the feasibility of the planning online.

[0016] II. Technical Concept

[0017] The technical concept of this invention is as follows:

[0018] To address the difficulties in online planning caused by pallet pose deviations in flexible pickup and delivery scenarios, this invention proposes a three-level progressive trajectory planning strategy: offline prediction, initial search solution, and step-by-step optimization.

[0019] The first level is the offline prediction level. Its core idea is to move the feasibility exploration of a large number of different pallet deviation conditions in online real-time planning to the offline stage. By systematically traversing the pallet deviation parameter space in the offline simulation environment, a planarable parameter space lookup table is established using lateral offset, longitudinal offset, and yaw angle as three-dimensional indices. The planarability state corresponding to each combination of pallet deviations and the required pre-adjustment driving strategy are pre-stored. During online operation, only the currently perceived pallet deviation needs to be used as an index to look up the table to obtain the feasibility judgment and pre-adjustment strategy in a very short time. This avoids performing invalid searches on infeasible deviation combinations. At the same time, pre-adjustment straight driving expands the vehicle attitude adjustment space to improve the planning success rate.

[0020] The second stage is the initial solution search stage. Its core idea is to use a hybrid A* algorithm combined with Reeds-Shepp curves for path search after confirming planarability and determining the start and end points of the main trajectory. However, instead of simply using the shortest geometric length as the selection criterion in existing solutions, a penalty mechanism based on the number of driving direction switches is introduced. The technical principle of this mechanism is as follows: for cargo AGVs, reversing itself does not have a significant negative impact, but frequent switching between forward and reverse causes the vehicle to repeatedly decelerate, stop, and start, increasing execution time and energy consumption. More importantly, it causes the cargo on the forks to slip or even tip over due to repeated changes in the direction of inertial forces. Therefore, this invention superimposes a penalty term proportional to the number of direction switches into the Reeds-Shepp curve cost function, prioritizing the path with fewer switches among multiple candidate paths of similar geometric length. After obtaining the initial path solution, a lightweight pre-optimization method based on gradient descent is used to initially improve curvature and smoothness, providing an initial solution with a more uniform curvature distribution and less high-frequency oscillation for subsequent deep optimization at a relatively low computational cost. This pre-optimization introduces a step size normalization factor to address the gradient distortion problem caused by the inconsistent sampling interval between the conventional extended segment and the analytical connection segment in the hybrid A* output path.

[0021] The third level is the deep optimization level. Its core idea is to use cubic B-spline curves as the parameterized expression of the trajectory, transforming the trajectory optimization problem into a multi-constraint nonlinear optimization problem for B-spline control points. The technical principle of choosing cubic B-splines is that cubic B-spline curves naturally possess second-order continuity in the parameter domain. Their curvature changes continuously throughout the entire curve and can be analytically calculated using the coordinates of the control points. Furthermore, the local support of the B-spline ensures that modifying a single control point only affects the local curve segment, which is beneficial for local adjustments without causing global oscillations. In the optimization objective, a fit preservation term is set to prevent the curve from deviating excessively from the pre-optimized initial path solution; a curvature upper limit penalty term is set to ensure that the trajectory is within the vehicle's minimum turning radius capability; an obstacle safety distance penalty term is set to ensure that the vehicle's safety envelope does not intrude into the obstacle area; a high-order smoothness term is set to suppress sudden acceleration changes, thereby improving trajectory tracking comfort and executability; and an endpoint straight-line constraint term is set to ensure that the heading and curvature at the end of the main trajectory are consistent with the straight-line fork entry segment, thus meeting the strict engineering requirements for fork and pallet fork slot alignment. The aforementioned multiple constraints are weighted and summed to form a unified objective function, which is then iteratively solved for the control points using a quasi-Newton optimization method.

[0022] III. Technical Solution

[0023] To address the aforementioned technical problems, this invention provides a trajectory planning and optimization method for flexible AGV cargo handling scenarios, comprising the following steps.

[0024] Step 1: Select the start and end points for main trajectory planning. Obtain the pallet's pose information relative to the AGV. Use the pallet's lateral offset, longitudinal offset, and deflection angle as indexes to query the planarable parameter space lookup table. Based on the query results, determine whether to perform pre-adjustment straight-line driving and determine the start point for main trajectory planning. Using the pallet pick-up / placement target pose as a reference, backtrack a preset distance in the opposite direction of the straight-line fork entry direction to determine the end point for main trajectory planning.

[0025] Step 2, Path Search and Pre-optimization. Starting from the main trajectory planning start point and ending at the main trajectory planning end point, a hybrid A* algorithm incorporating a driving direction switching penalty is used to search for an initial path solution. Subsequently, the path is segmented based on the driving direction indicators of the motion primitives within the path, and curvature and smoothing pre-optimization is performed on each constant-direction sub-segment.

[0026] Step 3: Trajectory Fitting and Optimization. The arc length of each sub-segment is parameterized and fitted using cubic B-spline curves. For the B-spline control points of each sub-segment, a multi-objective penalty function is performed to optimize under penalty terms including fit preservation, upper curvature limit, obstacle safety distance, and higher-order smoothness. For the last sub-segment connecting the straight-ahead fork segment, an endpoint straight-ahead penalty term is also included. After optimization convergence, the upper curvature limit and safety distance are verified. If the verification passes, equal arc length sampling is performed, and the trajectory is spliced ​​with the straight-ahead fork segment to form the final trajectory.

[0027] IV. Beneficial Effects

[0028] Compared with the prior art, the present invention provides specific solutions to the five types of technical defects described in the background art, and the resulting beneficial effects correspond one-to-one with each type of defect, as detailed below:

[0029] To address the first defect (insufficient kinematic modeling): This invention ensures that the trajectory satisfies the non-holonomic constraints of the fork-type AGV. During the search phase, this invention directly expands the state grid points using a bicycle kinematic model that includes the vehicle's equivalent wheelbase L_w, steering angle δ_k, and driving direction marker σ_k. Each candidate motion primitive naturally satisfies the minimum turning radius constraint. In the optimization phase, the maximum allowable curvature κ_max of the vehicle is further used as a hard check threshold. For points exceeding the limit, a piecewise quadratic penalty function ψ(·) is used for iterative iteration until the global curvature is acceptable. The resulting trajectory is within the vehicle's physical executable range at every sampling point, avoiding the sharp corners and untrackable problems inherent in grid or topology graph-based methods. It can be directly delivered to the vehicle controller for execution without secondary pruning.

[0030] To address the second defect (lack of direction switching cost): This invention explicitly suppresses the number of direction switching to protect cargo stability. Innovatively, this invention introduces a penalty term proportional to the number of direction switching, *n*, into the Reeds-Shepp analytical connection cost function, constructing a corrected cost C_RS = L × (1 + *w_dir* × *n*). Simultaneously, a direction switching penalty is applied in the regular extended segment when the parent and child nodes have inconsistent driving directions. This mechanism allows the hybrid A* search to prioritize paths with fewer direction switching among multiple candidate paths of similar geometric length, reducing the timing overhead of deceleration-stopping-shifting-restarting and the inertial force reversal event from the source. This significantly reduces the risk of pallet cargo slippage, misalignment, and tipping during cargo loading, and is particularly effective for fragile and high-center-of-gravity cargo. In the field test of Scenario 1, this mechanism reduced the number of direction switching from the common 2 to 3 times in existing methods to 0 times.

[0031] To address defect three (optimization instability caused by non-uniform sampling): This invention specifically handles gradient distortion at the junction of mixed A* sequences. This invention introduces a step size normalization factor during the pre-optimization stage:

[0032] η_j=max(La_j,Lb_j)^2 / (La_j×Lb_j+ε_s^2);

[0033] The gradient of the curvature penalty term is divided by η_j and then used for path point updates. When La_j and Lb_j are close, η_j approaches 1 and does not cause significant scaling. However, when they differ significantly, η_j is significantly greater than 1, thus suppressing pseudo-gradient amplification in short-spacing segments. This mechanism specifically addresses the non-uniform sampling problem at the junction of the mixed A* conventional extended segment (fixed arc length Δs) and the Reeds-Shepp analytical connection segment (independent sampling density), avoiding local distortion at the junction of the optimized path, a capability not possessed by existing smoothing methods.

[0034] Regarding defect four (lack of a unified multi-constraint framework): This invention simultaneously satisfies the composite requirements of the picking and placing process under a unified objective function. This invention uses a cubic B-spline curve as the parameterization carrier to construct a unified multi-objective penalty function:

[0035] J_total=w1×J_stay+w2×J_curv+w3×J_obs+w4×J_smooth_bsp+w5×J_end;

[0036] This approach incorporates four engineering requirements—fit preservation, curvature upper limit, obstacle safety distance, and high-order smoothness—into a single optimization problem. The final segment connecting the straight-line fork entry is further constrained by an endpoint straight-line constraint term, which is then solved in one step using the L-BFGS quasi-Newton method. The endpoint straight-line constraint term, J_end = ||t_end - h_g||^2 + γ_end × κ_end^2, specifically addresses the unique engineering requirement of front-fork AGVs maintaining a straight-line posture at the end to align with the pallet fork slot. A hard constraint strategy of collinearity among the three control points at the end ensures that the second-order difference at the end is strictly zero. After optimization convergence, curvature and safety distance checks serve as fallbacks for pass / fail assessment. If checks fail, re-optimization is performed using a weighted boosting method, and an upper limit counter N_retry is set to prevent infinite loops. This unified framework avoids the problem of mutual sacrifice between steps in existing multi-step serial processing, ensuring that the final trajectory quality meets the standards simultaneously under multi-dimensional constraints.

[0037] Regarding defect five (lack of feasibility prediction): This invention moves the planning feasibility boundary to the offline stage for characterization. In the offline simulation environment, this invention systematically traverses the pallet deviation parameter space using (Δx, Δy, Δθ) triples as indices, constructing a lookup table for the planarable parameter space. Each discrete combination is pre-labeled as state A (directly planarable), state B (planarable after pre-adjustment), or state C (unplanarable). For state B, the recommended pre-adjustment distance d_adv and direction σ_adv are further stored. During online runtime, only the perceived pallet deviation triples need to be looked up in the table to complete the feasibility judgment and pre-adjustment strategy issuance within milliseconds, avoiding the post-event error mode of performing a complete hybrid A* search (hundreds of milliseconds to seconds) on state C combinations only to find no solution. This mechanism concentrates the feasibility exploration work, originally scattered in each online run, into a single offline stage, significantly reducing the probability of production line downtime and wasted computing power.

[0038] The above five beneficial effects correspond to five types of technical defects, forming a complete mapping relationship. The three-level progressive strategy (offline prediction - initial search solution - step-by-step optimization) and the multi-constraint unified optimization framework together ensure that the trajectory output by this invention has five engineering characteristics: feasibility and predictability, fewer reversal times, stable optimization values, parallel satisfaction of multiple constraints, and docking with end-process engineering. It is suitable for automatic picking and placing of goods by front-fork AGVs in warehousing and logistics scenarios where pallets have multi-dimensional pose deviations. Attached Figure Description

[0039] Figure 1 This is a schematic diagram of the overall process of the method of the present invention.

[0040] Figure 2 This is a schematic diagram illustrating the offline construction and online querying of a planar parameter space lookup table.

[0041] Figure 3 A schematic diagram showing the selection of the starting point and ending point of the main trajectory planning.

[0042] Figure 4 This is a schematic diagram showing how a path is divided into segments according to the direction of travel.

[0043] Figure 5 This is a schematic diagram of cubic B-spline curve fitting and multi-constraint joint optimization.

[0044] Figure 6 This is a schematic diagram showing the final planned trajectory and the connection between the straight-ahead fork segment. Detailed Implementation

[0045] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. The following embodiments are used to explain the technical solutions of the present invention and are not intended to limit the scope of protection of the present invention. In this embodiment, the calculation of angles is uniformly performed in radians.

[0046] I. Definition of Coordinate System, Input Quantities, and Vehicle Parameters

[0047] This embodiment uses a front-fork AGV as an example. The planning reference point for the AGV can be selected as the rear axle center, the vehicle's geometric center, or other calibrated control reference points. This embodiment uses the rear axle center as the planning reference point.

[0048] Establish a local coordinate system for the AGV, with the origin as the AGV planning reference point, the X-axis pointing forward of the vehicle, and the Y-axis pointing to the left side of the vehicle. The heading angle θ is zero in the positive direction of the X-axis and positive in the counterclockwise direction. All points used for path planning and trajectory optimization can be calculated in the AGV local coordinate system or the unified map coordinate system; when multiple coordinate systems are involved, they should first be transformed to the same coordinate system through coordinate transformation.

[0049] The main parameters of the AGV are as follows:

[0050] The vehicle has a body length of 2.0 meters, a body width of 1.2 meters, a fork length of 1.15 meters, an equivalent wheelbase of 1.0 meter, a minimum turning radius of 1.5 meters, and a corresponding maximum permissible curvature κ_max of 0.667 meters per meter. The maximum forward speed is 1.5 meters per second, and the maximum reverse speed is 0.8 meters per second. These parameters are for illustrative purposes only and should be calibrated according to the specific vehicle model in actual applications.

[0051] The perception module, consisting of LiDAR, vision sensors, or a combination of both, is used to identify the pallet's pose relative to the AGV. The pallet pose includes the target position p_p and the straight-line fork-entry direction θ_g. p_p = (x_p, y_p), where x_p and y_p are the two planar coordinates of the target position of the pallet. θ_g is the vehicle heading angle when the AGV enters the pallet fork slot in a straight line.

[0052] The tray deviation parameter used for table lookup is defined as follows:

[0053] Δx represents the lateral offset of the pallet relative to the standard pick-up and drop position, in meters;

[0054] Δy represents the longitudinal offset of the pallet relative to the standard pick-up and drop position, in meters;

[0055] Δθ represents the deflection angle of the pallet relative to the standard pick-up and drop posture. The unit is radians in the formula calculation, but degrees can be used in the engineering description.

[0056] The overall method flow of the present invention is as follows: Figure 1 As shown, the process includes three stages: Step 1, selection of the starting and ending points of the main trajectory planning; Step 2, path search and pre-optimization; and Step 3, trajectory fitting and optimization.

[0057] Step 1: Selection of the starting and ending points for main trajectory planning

[0058] 2.1 Construction of the Planeable Parameter Space Lookup Table

[0059] In scenarios involving flexible pallet retrieval and placement, the space around the storage location for AGVs to adjust their posture is limited. When the pallet shifts or deflects significantly, the AGV may not be able to directly plan a trajectory that meets the curvature and forklift entry requirements at the current storage location's forward point. Therefore, this invention constructs a planarable parameter space lookup table during the offline phase.

[0060] The process of constructing the programmable parameter space lookup table is as follows:

[0061] First, determine the discrete range of the pallet deviation parameters. In this embodiment, the lateral offset Δx ranges from -0.30 meters to 0.30 meters, with a step size of 0.05 meters; the longitudinal offset Δy ranges from -0.20 meters to 0.20 meters, with a step size of 0.05 meters; and the deflection angle Δθ ranges from -15 degrees to 15 degrees, with a step size of 2.5 degrees. These ranges and step sizes can be adjusted based on pallet recognition error, warehouse space dimensions, and vehicle capacity.

[0062] Second, a kinematic model of the target AGV, its safety envelope, fork dimensions, storage location boundaries, and obstacle boundaries are established in a simulation environment. Planning tests are performed for each discrete combination (Δx, Δy, Δθ).

[0063] Third, perform two rounds of planning tests for each discrete combination. In the first round, pre-adjusted straight-line travel is not enabled; the AGV is planned directly from the warehouse pre-position point to the starting point of the straight-line forklift section. In the second round, pre-adjusted straight-line travel is enabled; that is, the AGV is first moved a certain distance away from the pallet or the warehouse location in a safe direction before the main trajectory planning is executed.

[0064] Fourth, based on the planning and testing results, each discrete combination is labeled into three states:

[0065] The first state (state A) indicates that the straight-line driving plan can be successfully completed without pre-adjustment;

[0066] The second state (state B) indicates that pre-adjustment to straight driving is required before the planning can be successful;

[0067] The third state (state C) indicates that even if pre-adjusted straight-line driving is performed, planning still cannot be successful.

[0068] For state B, the lookup table further records the recommended pre-adjustment distance d_adv and the recommended pre-adjustment driving direction σ_adv. Here, d_adv is in meters, and σ_adv is the driving direction indicator; σ_adv=+1 indicates the vehicle is moving forward, and σ_adv=-1 indicates the vehicle is reversing. If, in an actual project, the direction away from the pallet is fixed as reversing, σ_adv can also be fixed at -1.

[0069] like Figure 2 As shown, the scalable parameter space lookup table is constructed in an offline simulation manner, and performs fast lookups using the pallet deviation parameters obtained by perception as an index during online runtime.

[0070] 2.2 Online Query and Determination of the Starting Point of Main Trajectory Planning

[0071] Once the AGV reaches the storage location's pre-position, the sensing module outputs the pallet deviation parameters (Δx, Δy, Δθ). The control system uses this triple as an index to query the programmable parameter space lookup table.

[0072] When the query result is status A, the AGV's current position and current heading are used as the starting point for the main trajectory planning. The starting point for the main trajectory planning is denoted as x_start, where x_start = (x_s, y_s, θ_s). Here, x_s and y_s are the starting plane coordinates, and θ_s is the starting heading angle.

[0073] When the query result is status B, the AGV first performs a pre-adjustment straight-line movement. The distance of the pre-adjustment straight-line movement is d_adv, and the direction is σ_adv. The pose after the pre-adjustment is used as the starting point for the main trajectory planning. The pre-adjustment straight-line movement can be forward or reverse, depending on the vehicle's current orientation, the parking space orientation, and the safety strategy; its essence is to increase the vehicle's posture adjustment space without collision. After the pre-adjustment is completed, the AGV reaches the new position (x_s', y_s', θ_s'), and this pose is recorded as the starting point for the main trajectory planning: x_start=(x_s', y_s', θ_s'); where x_s'=x_s+σ_adv×d_adv×cosθ_s, y_s'=y_s+σ_adv×d_adv×sinθ_s, and θ_s'=θ_s.

[0074] When the query result is status C, the control system reports an unplannable anomaly to the upper-level scheduling system, indicating that the pallet offset or deflection exceeds the current AGV's capacity range, requiring manual or other equipment adjustments, and terminating the current planning.

[0075] 2.3 Determination of the endpoint of the main trajectory planning

[0076] To avoid symbol ambiguity, the following conventions apply: p_goal=(x_g,y_g) represents the two-dimensional position coordinates of the endpoint of the main trajectory planning; x_goal=(x_g,y_g,θ_g) represents the complete pose of the endpoint of the main trajectory planning, where θ_g is the straight-line fork direction angle. p_goal is the position component of x_goal.

[0077] Front-fork AGVs must maintain a strictly straight path when finally entering the pallet fork slot. To meet this requirement, this invention sets the endpoint of the main trajectory planning as the starting point of the straight-line fork entry segment.

[0078] Define the unit vector h_g for the straight-ahead fork direction as:

[0079] h_g=(cosθ_g,sinθ_g);

[0080] Where θ_g is the vehicle heading angle corresponding to the straight-ahead fork direction; cosθ_g represents the cosine value of θ_g; and sinθ_g represents the sine value of θ_g.

[0081] The preset fork entry distance d_in is calculated as follows:

[0082] d_in = L_fork + d_clear + d_ref;

[0083] Where L_fork is the fork extension length, d_clear is the safety clearance between the fork tip and the pallet, and d_ref is the longitudinal distance from the AGV planning reference point to the root of the fork.

[0084] The position coordinates p_goal of the endpoint of the main trajectory planning are determined by the following formula:

[0085] p_goal = p_target - d_in × h_g;

[0086] Where p_target is the coordinate of the target position for pallet pickup / pickup, i.e., p_target = p_p.

[0087] Therefore, we can see that moving d_in along the h_g direction from p_goal will reach p_target, that is, p_target = p_goal + d_in × h_g.

[0088] In this embodiment, L_fork is 1.15 meters, d_clear is 0.10 meters, d_ref is 1.05 meters, therefore d_in is 2.30 meters.

[0089] like Figure 3 As shown, the AGV planning reference point is taken as the center of the rear axis, the starting point of the main trajectory x_start is located in the channel, and the ending point of the main trajectory x_goal is determined by tracing back d_in in the opposite direction of the target pose along the fork entry direction. The end point of the straight fork entry segment just reaches the target pose for pallet picking and placing.

[0090] III. Step Two: Path Search and Pre-optimization

[0091] 3.1 Hybrid A* Vehicle Kinematics Model

[0092] This invention employs a hybrid A* algorithm to search for paths that satisfy incompleteness constraints. The AGV kinematic model can be approximated using a bicycle model. For the k-th discrete state, the vehicle state is (x_k, y_k, θ_k), the expansion step size is Δs, the steering angle is δ_k, and the driving direction indicator is σ_k. The subsequent state is then calculated using the following formula:

[0093] x_{k+1}=x_k+σ_k×Δs×cosθ_k;

[0094] y_{k+1}=y_k+σ_k×Δs×sinθ_k;

[0095] θ_{k+1}=θ_k+σ_k×Δs×tanδ_k / L_w;

[0096] Where x_k and y_k are the planar coordinates of the k-th state; θ_k is the vehicle heading angle of the k-th state; x_{k+1}, y_{k+1}, and θ_{k+1} are the successor states; σ_k=+1 indicates forward movement, and σ_k=-1 indicates reverse movement; Δs is the single-step arc extension; δ_k is the steering angle; tanδ_k is the tangent of δ_k; and L_w is the vehicle's equivalent wheelbase.

[0097] In this embodiment, Δs is set to 0.30 meters, and the steering angle is discretized into five types: large left turn, small left turn, straight, small right turn, and large right turn. Each steering angle is combined with two driving directions: forward and reverse, generating a total of ten candidate extended motion primitives.

[0098] 3.2 Improved Reeds-Shepp Curve Cost Function

[0099] During the hybrid A* path search process, when a search node satisfies the parse expansion trigger condition, the Reeds-Shepp connection path from the current node to the target node is calculated, and the merits of the connection path are evaluated using an improved cost function. For a candidate Reeds-Shepp curve, after removing motion primitive segments of zero length, M_rs non-zero motion primitive segments are obtained. Each non-zero motion primitive segment has a sign length l_q, q=1,2,...,M_rs. The total geometric length L is calculated using the formula L=sum|l_q|, where the summation range is from q=1 to M_rs, and |l_q| is the absolute value of the sign length of the q-th non-zero motion primitive segment. The number of direction switching n is calculated using the formula n=sumI(sign(l_q)≠sign(l_{q-1})), where the summation range is from q=2 to M_rs, sign(·) is the sign function, and I(·) is the indicator function. I takes the value 1 when the condition within the parentheses is true, and 0 otherwise.

[0100] The improved Reeds-Shepp curve cost function is as follows:

[0101] C_RS = L × (1 + w_dir × n);

[0102] Where L is the total arc length of the Reeds-Shepp connection path in meters; n is the number of direction changes in the connection path, a non-negative integer; w_dir is the direction change penalty weight, a dimensionless positive coefficient ranging from 0.5 to 5.0. Smaller w_dir values ​​result in weaker suppression of direction changes, while larger values ​​lead to a stronger preference for paths with fewer direction changes. In this embodiment, w_dir is set to 2.0. This cost function is used for evaluating and filtering parsed extended candidate paths in the hybrid A* algorithm. More direction changes result in a higher cost, thus guiding the search to prioritize paths with fewer direction changes.

[0103] 3.3 Hybrid A* Search Process

[0104] The hybrid A* search process is as follows.

[0105] First, x_start is added to the set of open nodes, and its cumulative cost g is set to 0. The heuristic cost h is the shortest Reeds-Shepp geometry from x_start to x_goal, excluding the direction switching penalty. Since the true cost g* satisfies g*=L*×(1+w_dir×n*)≥L*≥L_RS_min (L_RS_min is the shortest Reeds-Shepp geometry, n*≥0), h≤g*. The heuristic value does not overestimate the true optimal cost, satisfying the allowable property of the A* algorithm, thus ensuring the optimality of the search result.

[0106] Second, select the node with the minimum total cost f from the set of open nodes as the current node. The total cost f is calculated according to the formula:

[0107] f = g + h;

[0108] Where g is the cumulative cost from the starting point to the current node, and h is the heuristic cost from the current node to the target node.

[0109] Third, candidate successor nodes are generated according to the vehicle kinematics model, and collision detection is performed on each candidate motion primitive. During collision detection, the AGV vehicle outline, fork outline, and safety margin are merged into a vehicle safety envelope, and it is checked whether this safety envelope intersects with obstacles or storage space boundaries along the sweep area of ​​the motion primitive.

[0110] Fourth, calculate the cumulative cost for collision-free candidate nodes. The cumulative cost may include motion length, steering cost, and direction switching penalty. If the travel direction of a candidate node is different from the motion primitive direction of its parent node, a direction switching penalty is added.

[0111] Fifth, every time the search is extended a preset number of times or the distance between the current node and the target node is less than a preset threshold, an attempt is made to connect the current node and x_goal using a Reeds-Shepp curve with improved cost. If the connection curve resolves without collision, the search is successful.

[0112] Sixth, when the search is successful, the parent node is traced back to obtain the initial path solution P_Astar. The initial path solution consists of N discrete path points, each of which includes its position, heading angle, and direction of travel. If the open node set is empty and no feasible path is found, the planning fails and an anomaly is reported.

[0113] 3.4 Divide into sub-segments according to driving direction signs

[0114] like Figure 4 As shown, the initial path solution is divided into several segments with constant driving direction based on the driving direction marker σ. The common connection point between adjacent segments is the driving direction switching point. After obtaining the initial path solution P_Astar, it is segmented according to the driving direction marker σ saved during the hybrid A* search. σ=+1 indicates forward movement, and σ=-1 indicates reverse movement.

[0115] The process sequentially checks adjacent motion primitive segments along the path. If the σ values ​​of adjacent motion primitive segments are different, the common connection point is marked as a driving direction switching point. The path between two adjacent driving direction switching points constitutes a driving direction constant sub-segment.

[0116] Using driving direction markers to segment the data is more reliable than simply using the inner product of adjacent displacement vectors. This is because in curves with significant curvature, the angle between adjacent displacement vectors may be large, even though the vehicle has not switched between forward and reverse movement; while driving direction markers, derived directly from motion elements, can accurately reflect the vehicle's direction of travel.

[0117] If the route data does not include driving direction markers, it can be estimated using the following formula:

[0118] σ_j=sign((p_j-p_{j-1})·h_j);

[0119] h_j=(cosθ_j,sinθ_j);

[0120] Where σ_j is the estimated driving direction of the j-th sampling point; p_j is the position vector of the j-th path point; p_{j-1} is the position vector of the (j-1)-th path point; h_j is the vehicle heading unit vector at the j-th path point; θ_j is the vehicle heading angle; the symbol “·” represents the vector dot product. When the displacement vector is in the same direction as the heading, the dot product is positive, and σ_j is +1 to indicate forward movement; when the displacement vector is opposite to the heading, the dot product is negative, and σ_j is -1 to indicate reverse movement. When the absolute value of the dot product (p_j-p_{j-1})·h_j is less than the preset threshold ε_σ (1×10^(-4) meters in this embodiment), it is considered that a turn in place or displacement degradation has occurred at that point, and σ_j follows the σ_{j-1} of the previous sampling point; if j=1, i.e., the first point is degraded, then σ_1=+1 (default forward movement).

[0121] 3.5 Gradient Descent Pre-Optimization Objective

[0122] Perform pre-optimization independently for each segment with a constant driving direction. Suppose a segment contains N_m path points, with path point positions p_j, p_j=(x_j,y_j), j=0,1,...,N_m-1.

[0123] During pre-optimization, the start and end points of the sub-segments are fixed. To maintain the endpoint heading, the first point after the start point and the first point before the end point can also be fixed, or a high-weight penalty can be applied to the endpoint tangent. The pre-optimization objective function J_pre is:

[0124] J_pre=β_curv×J_curv_pre+β_smooth×J_smooth_pre+β_ref×J_ref_pre;

[0125] Wherein, J_curv_pre is the pre-optimized curvature constraint term; J_smooth_pre is the pre-optimized smoothing term; J_ref_pre is the reference preservation term, used to prevent the path from deviating excessively from the initial search solution; β_curv, β_smooth, and β_ref are the weights of the three terms. In this embodiment, β_curv is set to 1.0, β_smooth to 0.3, and β_ref to 0.1.

[0126] The reference retention term J_ref_pre is:

[0127] J_ref_pre=sum||p_j-p_j0||^2;

[0128] Where p_j0 is the initial position of the j-th path point before pre-optimization, p_j is the position of the j-th path point during the optimization process, and ||p_j-p_j0|| represents the Euclidean distance between them.

[0129] 3.6 Discrete Curvature Calculation and Curvature Constraint Terms

[0130] For the j-th intermediate path point in the sub-segment, the discrete curvature κ_j is calculated using the following formula:

[0131] a_j = p_j - p_{j-1};

[0132] b_j = p_{j+1} - p_j;

[0133] La_j=sqrt((x_j-x_{j-1})^2+(y_j-y_{j-1})^2);

[0134] Lb_j=sqrt((x_{j+1}-x_j)^2+(y_{j+1}-y_j)^2);

[0135] Define the cross product scalar Cr_j = a_x_j × b_y_j - a_y_j × b_x_j, and the inner product scalar Dot_j = a_x_j × b_x_j + a_y_j × b_y_j; then φ_j = atan2(Cr_j, Dot_j);

[0136] κ_j=|φ_j| / (0.5×(La_j+Lb_j)+ε_s);

[0137] Where p_j=(x_j,y_j) are the planar coordinates of the j-th path point; a_j is the displacement vector from the (j-1)-th point to the j-th point, a_x_j and a_y_j are its x-component and y-component respectively; b_j is the displacement vector from the j-th point to the (j+1)-th point, b_x_j and by_j are its x-component and y-component respectively; La_j is the length of the first segment; Lb_j is the length of the second segment; φ_j is the signed angle between a_j and b_j; atan2 is the four-quadrant arctangent function, whose first parameter is the cross product of two-dimensional vectors a_x_j multiplied by b_y_j minus a_y_j multiplied by b_x_j, and the second parameter is the inner product of vectors a_x_j multiplied by b_x_j plus a_y_j multiplied by b_y_j; ε_s is the positive threshold to prevent division by zero, which is 0.001 meters in this embodiment. The value of j ranges from 1 to N_m-2, where N_m is the total number of path points in the current sub-segment.

[0138] The pre-optimized curvature constraint term J_curv_pre is:

[0139] J_curv_pre=sumψ(κ_j-κ_max), j=1,2,...,N_m-2;

[0140] Where κ_max is the maximum allowable curvature of the vehicle; ψ(z) is a piecewise quadratic penalty function, which is defined as follows: when z is less than or equal to 0, ψ(z) = 0; when z is greater than 0, ψ(z) = z^2.

[0141] 3.7 Step Size Normalization Factor

[0142] Hybrid A* paths are typically formed by splicing regular extended segments and Reeds-Shepp analytical connect segments, and the sampling intervals between different segments may vary. If the curvature gradient is calculated directly on non-uniform sampling points, abnormally large gradients may be generated in short-distance segments, leading to local distortion of the path.

[0143] Therefore, this invention introduces a step size normalization factor η_j:

[0144] η_j=max(La_j,Lb_j)^2 / (La_j×Lb_j+ε_s^2);

[0145] Where η_j is the step size normalization factor of the j-th path point; La_j and Lb_j are the lengths of the two segments before and after the j-th path point, respectively; max(La_j,Lb_j) means taking the larger value of La_j and Lb_j and squaring it; ε_s is the positive threshold to prevent division by zero.

[0146] When non-uniform sampling exists in the path (such as at the junction of a regular extended segment and an analytical connection segment), the step size normalization factor η_j can effectively suppress abnormal gradient amplification in short step size segments, avoiding local distortion of the optimized path. When La_j and Lb_j are equal, η_j is close to 1, and no significant scaling of the gradient is applied; when La_j and Lb_j differ significantly, η_j is greater than 1, and dividing the gradient of the curvature penalty term by η_j can suppress abnormal gradients caused by non-uniform sampling.

[0147] 3.8 Five-point difference smoothing term

[0148] To suppress high-frequency oscillations in the path, this invention employs a five-point difference smoothing term. For the j-th path point, the five-point difference smoothing term D_j is defined as:

[0149] D_j=p_{j-2}-4×p_{j-1}+6×p_j-4×p_{j+1}+p_{j+2};

[0150] Where p_j is the coordinate vector of the j-th path point; the value of j ranges from 3 to N_m-3, and N_m is the total number of path points in the current sub-segment. For boundary points where j is less than 3 or greater than N_m-3, the five-point difference smoothing term is not calculated, and its smoothness is guaranteed by the endpoint constraints. The pre-optimized smoothing term J_smooth_pre is the sum of squares of the magnitudes of the five-point difference vectors of all valid index points: J_smooth_pre=sum(||D_j||^2), j=3,4,...,N_m-3;

[0151] Where D_j reflects the degree of higher-order oscillation of the local path, and ||D_j|| is its Euclidean norm.

[0152] When the minimum distance between the j-th path point and its adjacent path points is less than the preset minimum step size threshold d_min, the smooth gradient of that point is set to zero or point merging is performed. In this embodiment, d_min is set to 0.05 meters.

[0153] 3.9 Pre-optimization iterative update

[0154] Calculate the total gradient grad_j for each optimizable path point:

[0155] grad_j=β_curv×grad_curv_j / η_j+β_smooth×grad_smooth_j+β_ref×grad_ref_j;

[0156] Where grad_j is the total gradient of the j-th path point; grad_curv_j is the gradient of the curvature constraint term; η_j is the step size normalization factor; grad_smooth_j is the gradient of the smoothing term; and grad_ref_j is the gradient of the reference preservation term.

[0157] The gradient grad_ref_j of the reference preservation term with respect to the j-th path point is:

[0158] grad_ref_j=2×(p_j-p_j0);

[0159] Where p_j is the position of the j-th path point in the current iteration, and p_j0 is the initial position of the j-th path point before pre-optimization.

[0160] The path point update formula is:

[0161] p_j_new=p_j-α×grad_j;

[0162] Where p_j_new represents the updated path point; p_j represents the path point before the update; and α represents the learning rate. In this embodiment, the initial value of α is 0.1, and it gradually decreases with the number of iterations.

[0163] After each update, boundary checks and collision checks are performed on the path points. If the update causes a path point or local line segment to enter the obstacle safe zone, the update is rejected or the learning rate is reduced and the update is repeated. The pre-optimization termination condition is: the gradient norm of all optimizable points is less than the convergence threshold, or the maximum number of iterations is reached. In this embodiment, the convergence threshold is 0.001, and the maximum number of iterations is 200.

[0164] IV. Step 3: Cubic B-spline fitting and joint optimization

[0165] 4.1 Sub-segment arc length parameterization

[0166] For the pre-optimized m-th sub-segment, let it contain N_m path points p_0, p_1, ..., p_{N_m-1}. First, calculate the cumulative arc length:

[0167] s_0=0;

[0168] s_j=s_{j-1}+sqrt((x_j-x_{j-1})^2+(y_j-y_{j-1})^2), j=1,2,...,N_m-1;

[0169] Where s_j is the cumulative arc length from the starting point of the sub-segment to the j-th path point.

[0170] Normalize the cumulative arc length to the parameter u_j:

[0171] u_j=s_j / s_{N_m-1};

[0172] where u_j is the dimensionless parameter corresponding to the j-th path point, and u_j belongs to the closed interval from 0 to 1. If the distance between adjacent path points is smaller than the merging threshold d_merge, one of the points is retained and the other is deleted to improve the fitting stability. In this embodiment, d_merge is 0.02 meters.

[0173] 4.2 Definition of cubic B-spline curve

[0174] The present invention uses a cubic B-spline curve to fit each constant driving direction sub-segment. Let the control points be Q_0, Q_1, ..., Q_K, where Q_i=(X_i,Y_i), i is the control point index, K is the index of the last control point, and the total number of control points is K+1.

[0175] The cubic B-spline curve C(u) is defined as:

[0176] C(u)=sumN_{i,3}(u)×Q_i, i=0,1,...,K;

[0177] where C(u) is the curve point at the parameter u; N_{i,3}(u) is the value of the i-th cubic B-spline basis function at the parameter u; Q_i is the i-th control point; u is the curve parameter.

[0178] The B-spline basis function is calculated using the Cox-deBoor recurrence formula. The zero-order basis function is defined as:

[0179] When U_i≤u<U_{i+1}, N_{i,0}(u)=1; otherwise N_{i,0}(u)=0;

[0180] where U_i and U_{i+1} are adjacent knot values in the knot vector, and N_{i,0}(u) is the value of the i-th zero-order basis function at the parameter u.

[0181] The recurrence formula for the p-th order basis function is:

[0182] N_{i,p}(u)=((u-U_i) / (U_{i+p}-U_i))×N_{i,p-1}(u)+((U_{i+p+1}-u) / (U_{i+p+1}-U_{i+1}))×N_{i+1,p-1}(u);

[0183] Where N_{i,p}(u) is the value of the i-th p-th basis function at parameter u; N_{i,p-1}(u) is the value of the i-th p-1-th basis function at parameter u; N_{i+1,p-1}(u) is the value of the (i+1)-1-th basis function at parameter u; p is the degree of the basis function, and in this invention p=3; U_i, U_{i+p}, U_{i+1} and U_{i+p+1} are the node values ​​at the corresponding index positions in the node vector; when the denominator is 0, the corresponding fraction term is 0.

[0184] 4.3 Node Vector Construction

[0185] A uniform nodal vector with repeated endpoints is used. For a cubic B-spline, the length of the nodal vector U is K+5. The nodal vector is constructed as follows:

[0186] U_0=U_1=U_2=U_3=0;

[0187] U_{K+1}=U_{K+2}=U_{K+3}=U_{K+4}=1;

[0188] For internal nodes, i=4,5,...,K, set according to the formula:

[0189] U_i = (i-3) / (K-2);

[0190] Where U_i is the value of the i-th node. This endpoint repeating node vector ensures that the curve's starting point passes through Q_0 and its ending point passes through Q_K.

[0191] 4.4 Determination of Initial Control Points

[0192] The number of control points is determined based on the segment length and path complexity. In this embodiment, the number of control points can be one-third to one-half of the number of path points, ensuring that there are at least 6 control points. If a zero-curvature constraint needs to be applied at the end, the number of control points should be such that at least three control points can be set at the end.

[0193] Let the current segment's driving direction indicator be σ_m, where σ_m = +1 indicates a forward segment and σ_m = -1 indicates a reverse segment. The vehicle's heading unit vector is h = (cosθ, sinθ). The forward direction of the curve parameters should be consistent with the vehicle's actual direction of movement. Since the vehicle moves in the opposite direction of its heading when reversing, the direction of increase in the curve parameters should be consistent with the vehicle's actual direction of movement. Therefore, the tangential unit vector r of the curve's execution direction is defined as:

[0194] When σ_m = +1, r = h;

[0195] When σ_m=-1, r=-h;

[0196] Where r is the tangential unit vector along the execution sequence of the curve; h is the vehicle heading unit vector.

[0197] The control point initialization method is as follows.

[0198] First, endpoint position constraints:

[0199] Q_0 = p_0;

[0200] Q_K=p_{N_m-1};

[0201] Where p_0 is the starting point of the sub-segment and p_{N_m-1} is the ending point of the sub-segment.

[0202] Second, endpoint tangential constraints:

[0203] Q_1 = Q_0 + ρ_s × r_s;

[0204] Q_{K-1}=Q_K-ρ_e×r_e;

[0205] Where r_s is the tangential unit vector of the curve execution direction at the starting point of the sub-segment; r_e is the tangential unit vector of the curve execution direction at the ending point of the sub-segment; ρ_s and ρ_e are the endpoint tangential control distances.

[0206] Third, for the last main trajectory sub-segment connecting the straight-ahead fork segment, to ensure a smooth transition between the end of the main trajectory and the straight-ahead fork segment, the end tangential unit vector r_e in step two is replaced with the straight-ahead fork direction unit vector h_g, thus overriding the setting of Q_{K-1}; and an additional end-next-adjacent control point Q_{K-2} is added. Specifically:

[0207] Q_{K-1}=Q_K-ρ_e×h_g;

[0208] Q_{K-2}=Q_K-2×ρ_e×h_g;

[0209] Where h_g is the unit vector in the straight-line fork direction; ρ_e is the tangential control distance at the endpoint. By making Q_{K-2}, Q_{K-1}, and Q_K collinear at equal intervals along the h_g direction (with an interval of ρ_e), the second-order difference at the endpoint Q_{K-2}-2×Q_{K-1}+Q_K=0, thereby making the second derivative at the endpoint of the curve approach zero, and thus making the curvature at the endpoint approach zero.

[0210] Fourth, the remaining control points are sampled from the pre-optimized path to obtain initial values ​​according to their equal arc length positions.

[0211] In this embodiment, the average control point spacing obtained by dividing the total arc length of the segment by the number of control points is d_avg; ρ_s and ρ_e take values ​​in the range of 0.5×d_avg to 1.0×d_avg, and in this embodiment, ρ_s=ρ_e=0.7×d_avg is uniformly taken.

[0212] 4.5 Constrained Least Squares Fitting with Regularization

[0213] Construct a fitting matrix A, where the element in the j-th row and i-th column of matrix A is:

[0214] A_{j,i}=N_{i,3}(u_j);

[0215] Where A_{j,i} are matrix elements; u_j is the normalized arc length parameter of the j-th path sampling point; N_{i,3}(u_j) is the value of the i-th cubic B-spline basis function at u_j.

[0216] The fitting target J_ls with regularization is:

[0217] J_ls=sum||C(u_j)-p_j||^2+λ×sum||Q_{i+1}-2×Q_i+Q_{i-1}||^2;

[0218] Where C(u_j) is the point of the B-spline curve at u_j; p_j is the j-th reference point on the pre-optimized path; λ is the regularization coefficient, which is 0.001 in this embodiment; the second term is the second-order difference regularization term, which is used to suppress the violent oscillation of the control point.

[0219] Under endpoint position constraints, endpoint tangential constraints, and necessary end-point zero curvature constraints, the above-mentioned constrained least squares problem is solved to obtain the initial B-spline control points. This embodiment uses the elimination method to solve the problem. Specifically, the coordinate values ​​of the constrained control points (including endpoint position control points Q_0 and Q_K, end-point tangential control points Q_1 and Q_{K-1}, and control point Q_{K-2} involved in the end-point zero curvature constraint) are substituted into the fitting equation. After eliminating the corresponding columns in the fitting matrix A, the normal equation is solved for the remaining free control points:

[0220] (A_free^T×A_free+λ×R_free)×Q_free=A_free^T×(P_sample-A_fixed×Q_fixed);

[0221] Where A_free^T represents the transpose of A_free; A_free is a submatrix composed of columns corresponding to the free control points in the fitting matrix; A_fixed is a submatrix composed of columns corresponding to the fixed control points in the fitting matrix; Q_free is the coordinate matrix of the free control points to be solved; Q_fixed is the coordinate matrix of the fixed control points that have been determined; P_sample is the coordinate matrix of the sampling points; and R_free is a submatrix composed of rows and columns corresponding to the free control point indices in the second-order difference regularization matrix R of all control points. The full control point regularization matrix R is constructed as follows: Let the difference operator matrix M be a (K-1)×(K+1) matrix, and the non-zero elements of its i-th row (i=1,2,...,K-1) are: M[i,i-1]=1, M[i,i]=-2, M[i,i+1]=1, and the rest are zero; then R=Mᵀ×M, which is a (K+1)×(K+1) symmetric positive semi-definite matrix; let the number of free control points be n_free, and R_free be an n_free×n_free submatrix of R that takes the row and column corresponding to the free control point index.

[0222] 4.6 Optimization of Multi-Objective Penalty Function

[0223] For each pre-optimized sub-segment path point, arc length parameterization and cubic B-spline curve fitting are performed to obtain the corresponding B-spline control point sequence for each sub-segment. L-BFGS unconstrained optimization is then performed on the B-spline control points of each sub-segment, with each sub-segment independently solving its own multi-objective penalty function. For non-terminal sub-segments, endpoint position control points Q_0 and Q_K, as well as end tangential control points Q_1 and Q_{K-1}, are marked as invariant control points, and L-BFGS optimization is performed only on Q_2 to Q_{K-2}. For the last sub-segment connecting the straight-ahead fork segment, the invariant control points also include the end-second adjacent control point Q_{K-2}, narrowing the optimization range to Q_2 to Q_{K-3} (see Section 4.11 for details). For the connection points of adjacent sub-segments, positional and tangential continuity at the connection point is ensured through fixed endpoint positions and endpoint tangential constraints.

[0224] like Figure 5 As shown, during the optimization process, the B-spline curve is made to meet the requirements of safety margin, curvature and straight end by adjusting the position of the control points. The collinearity of the end control points ensures that the end curvature approaches zero.

[0225] The overall objective function J_total is:

[0226] J_total=w1×J_stay+w2×J_curv+w3×J_obs+w4×J_smooth_bsp+w5×J_end;

[0227] In this context, w1, w2, w3, and w4 are weight coefficients greater than 0; the endpoint straight-through constraint term w5×J_end only applies to the last sub-segment connecting the straight-through fork entry segment, and w5 is zero for the remaining sub-segments. In this embodiment, w1 is 1.0, w2 is 5.0, w3 is 10.0, w4 is 0.5, and w5 is 20.0 for the last sub-segment. The above weights can be adjusted according to vehicle speed, load, and scenario safety requirements.

[0228] The terms in the above multi-objective penalty function are all soft-constraint penalty terms. These are weighted sums to form an unconstrained optimization problem, which is solved using the L-BFGS algorithm. Since soft constraints cannot absolutely guarantee that curvature and safety distance meet hard limits, the following verification is performed on the output trajectory after optimization:

[0229] Curvature verification: Traverse all sampling points of the output trajectory and verify that the absolute value of curvature at each point does not exceed the maximum allowable curvature kappa_max, that is, |kappa_j|<=kappa_max holds true for all sampling points; if there are points that exceed the limit, increase the curvature penalty weight w2 and re-optimize.

[0230] Safety distance verification: Calculate the distance from all sampling points of the output trajectory to the nearest obstacle, and verify that the distance of each point is not less than the safety distance threshold d_safe, that is, dist_j>=d_safe holds true for all sampling points; if there are violation points, increase the obstacle penalty weight w3 and re-optimize.

[0231] Let N_retry be the combined retry limit counter for curvature verification and safe distance verification, with a value range of 2 to 5 times. After each verification failure, the corresponding weight is increased and the count is incremented by 1 when re-optimization is performed. When the cumulative count reaches the upper limit N_retry (3 times in this embodiment) and still fails, a planning failure exception is reported to the upper-level scheduling system to avoid infinite loop.

[0232] Only after the verification is passed can equal arc length sampling and straight-line fork segment splicing be performed.

[0233] 4.7 Fit Preservation Term

[0234] The mechanism of J_stay in this section is similar to that of the reference retention term J_ref_pre in the pre-optimization stage of Section 3.5 (both prevent the trajectory of the current stage from deviating from the output of the previous stage), but their objects of action are different: J_ref_pre directly acts on the position variable of the discrete path point p_j, with the initial solution as the reference object; J_stay acts on the curve sampling point C(v_l) implicitly driven by the control point Q_i, with the pre-optimized path as the reference object. They belong to different optimization stages and do not constitute redundancy.

[0235] The fit-preserving term J_stay is used to prevent the optimized curve from deviating excessively from the pre-optimized path.

[0236] J_stay=sum||C(v_l)-P_ref(v_l)||^2;

[0237] Where v_l is the l-th optimized sampling parameter; C(v_l) is the point of the B-spline curve at v_l; P_ref(v_l) is the reference point on the pre-optimized path corresponding to v_l, which is obtained by mapping v_l to an arc length ratio, that is, multiplying v_l by the total arc length of the sub-segment to obtain the target arc length value, locating the two adjacent points of the target arc length value in the cumulative arc length sequence of the pre-optimized path, and using linear interpolation to obtain the coordinates at that point; J_stay is the fit preservation cost.

[0238] 4.8 Curvature Penalty

[0239] At the parameter v_l, the first derivative of the B-spline curve is C'(v_l), and the second derivative is C''(v_l). Let C'(v_l) = (x'_l, y'_l) and C''(v_l) = (x''_l, y''_l). Define the parameter velocity V_l as the Euclidean norm of the first derivative:

[0240] V_l=sqrt(x'_l^2+y'_l^2);

[0241] The curvature κ_l is calculated according to the formula:

[0242] κ_l=|x'_l×y''_l-y'_l×x''_l| / max(V_l^3,ε_v);

[0243] Where x'_l and y'_l are the x and y components of the first derivative of the B-spline curve at v_l, respectively; x''_l and y''_l are the x and y components of the second derivative of the B-spline curve at v_l, respectively; V_l is the parametric velocity, i.e., the Euclidean norm of the first derivative vector; V_l^3 represents the cube of V_l; |x'_l×y''_l-y'_l×x''_l| is the absolute value of the two-dimensional cross product formed by the first and second derivatives; ε_v is the positive threshold to prevent division by zero, and its dimension is consistent with V_l^3. The magnitude of V_l^3 is s_total^3, and ε_v is (10^(-6)×s_total)^3, where s_total is the total arc length of the current B-spline segment (i.e., s_{N_m-1} in Section 4.1), used for dimension matching of ε_v.

[0244] The curvature penalty term J_curv is:

[0245] J_curv=sumψ(κ_l-κ_max);

[0246] Where κ_l is the curvature at the l-th sampling point; κ_max is the maximum allowable curvature of the vehicle; ψ(z) is a piecewise quadratic penalty function, where ψ(z) = 0 when z is less than or equal to 0, and ψ(z) = z^2 when z is greater than 0.

[0247] 4.9 Obstacle Safety Distance

[0248] This invention uses an obstacle map that has been expanded according to the outline of the AGV vehicle and the fork to query safe distances. In the expanded obstacle map, the distance d_obs_l returned for each query point represents the distance from that point to the nearest obstacle boundary after considering the vehicle's safety envelope.

[0249] The obstacle safety distance term J_obs is:

[0250] J_obs=sumψ(d_safe-d_obs_l);

[0251] Wherein, d_safe is the additional safety margin, with a value ranging from 0.10 meters to 0.50 meters, which needs to be determined comprehensively based on the AGV's external dimensions, the width of the storage space aisle, and the positioning accuracy; in this embodiment, d_safe is set to 0.20 meters; d_obs_l is the obstacle distance at the l-th sampling point; ψ(z) is a piecewise quadratic penalty function, where ψ(z) = 0 when z is less than or equal to 0 (i.e., no penalty is generated when the distance is greater than the safety margin), and ψ(z) = z^2 when z is greater than 0 (i.e., a penalty is generated when the distance is less than the safety margin, and the closer the distance, the greater the penalty). In this embodiment, d_safe is set to 0.20 meters.

[0252] If the obstacle map in the actual project does not have vehicle body expansion processing, the half width of the AGV vehicle, the outer dimensions of the forks and the extra margin should be included in the calculation of d_obs_l, and the safety envelope should be placed at C(v_l) and the corresponding heading to calculate the nearest obstacle distance.

[0253] 4.10 Higher-order smoothness terms

[0254] The higher-order smoothness term is represented by the third-order difference of four adjacent control points. The third-order difference vector jerk_i of the i-th group is:

[0255] jerk_i=Q_{i+3}-3×Q_{i+2}+3×Q_{i+1}-Q_i;

[0256] The higher-order smoothness term J_smooth_bsp is:

[0257] J_smooth_bsp=sum||jerk_i||^2,i=0,1,...,K-3;

[0258] Where jerk_i is the third-order difference vector of the i-th group; Q_i, Q_{i+1}, Q_{i+2} and Q_{i+3} are four consecutive control points; ||jerk_i|| represents the Euclidean norm of jerk_i; and K is the index of the last control point.

[0259] 4.11 Endpoint Straight-Ahead Constraints

[0260] A threshold ε_κ is defined to determine whether the curvature at the end of the main trajectory meets the requirement of approaching zero. In this embodiment, ε_κ is set to 0.005 per meter, while in engineering, its value ranges from 0.001 to 0.01 per meter. When |κ_end| < ε_κ, the end connection is considered to meet the second-order continuity requirement.

[0261] The last main trajectory segment needs to be connected with the straight-through fork segment. Define the end unit tangential t_end as: t_end=C'(1) / max(||C'(1)||,ε_t);

[0262] Where C'(1) is the first derivative of the B-spline curve at the endpoint parameter value 1; ||C'(1)|| is its Euclidean norm; ε_t is the positive threshold to prevent division by zero, and its dimensions are consistent with ||C'(1)||. Take ε_t=10^(-6)×s_total, where s_total is the total arc length of the current B-spline sub-segment.

[0263] The endpoint straight-line constraint term J_end is calculated according to the formula:

[0264] J_end=||t_end-h_g||^2+γ_end×κ_end^2;

[0265] Where h_g is the unit vector in the straight-line fork direction; t_end is the unit tangential vector at the end; ||t_end-h_g|| is the Euclidean norm of the difference between the two; κ_end is the curvature at the end of the curve, calculated in the same way as the curvature formula mentioned above; γ_end is the end curvature sub-weight within J_end, used to adjust the relative importance between the end tangential consistency objective and the end zero curvature objective. In this embodiment, γ_end is set to 10.0, and w5 is set to 20.0, so the equivalent total weight of the end curvature penalty is w5×γ_end=200.0, ensuring that the end curvature is strongly constrained to approach zero during the optimization process.

[0266] This embodiment employs a strategy that combines hard and soft constraints: during the initialization phase, Q_{K-2}, Q_{K-1}, and Q_K are forcibly set to be collinear and equally spaced along the h_g direction (hard constraint); in the L-BFGS optimization iteration, the endpoint control points Q_0 and Q_K, the end tangential control points Q_1 and Q_{K-1}, and the end-next adjacent control point Q_{K-2} are all marked as immutable control points (not participating in the optimization variables), and only Q_2 to Q_{K-3} are optimized; at the same time, the J_end soft constraint term is retained, so that the intermediate control points also tend to cooperate with the end tangential, improving the optimization stability. Under this strategy, the positions of the three end control points always satisfy Q_{K-2}-2×Q_{K-1}+Q_K=0, and the end curvature strictly approaches zero.

[0267] 4.12 Gradient Calculation and Optimization Solution

[0268] Since the B-spline curve points C(v_l) are a linear combination of control points Qi:

[0269] C(v_l)=sumN_{i,3}(v_l)×Q_i;

[0270] Therefore, for the cost term acting on the curve sampling points, its gradient with respect to the control point Q_i can be calculated using the chain rule:

[0271] dJ / dQ_i=sum(dJ / dC(v_l))×N_{i,3}(v_l)+G_direct_i;

[0272] Where dJ / dQ_i is the gradient of the total objective function with respect to the i-th control point; dJ / dC(v_l) is the partial derivative of the total objective function with respect to the l-th curve sampling point; N_{i,3}(v_l) is the value of the i-th cubic B-spline basis function at v_l; and G_direct_i is the gradient directly generated by the control point difference term (including the higher-order smoothness term and the part of the fit preservation term that is directly expressed with control points as variables) and the endpoint constraint term.

[0273] The curvature term involves the first and second derivatives, which can be analytically derived using the first and second derivatives of the B-spline basis functions and the chain rule; the gradient can also be calculated in engineering implementations using automatic differentiation or finite difference methods.

[0274] The optimization solution employs the L-BFGS quasi-Newton method. The L-BFGS method utilizes the BFGS approximation of the Hessian matrix information within limited memory to accelerate convergence, making it suitable for medium-sized optimization problems with tens to hundreds of control points. The optimization termination condition is: the relative rate of decrease of the objective function is less than a preset threshold, or the maximum number of iterations is reached. In this embodiment, the threshold for the relative rate of decrease of the objective function is set to 10^(-6), and the maximum number of iterations is set to 500.

[0275] V. Final Trajectory Output

[0276] After optimization and convergence, equal arc length sampling is performed on each B-spline segment. Specifically, dense sampling is first performed within the parameter range of 0 to 1 (1000 uniform parameter points are used in this embodiment), the distance between adjacent sampling points is calculated, and the arc length is accumulated; then, arc length interpolation is performed according to the target output spacing δ_out. In this embodiment, δ_out is set to 0.05 meters.

[0277] For each output trajectory point, calculate the vehicle heading angle based on the driving direction sign of the sub-segment containing that point. Let the tangent vector of the curve execution direction be τ, where τ = (τ_x, τ_y).

[0278] When the forward segment σ_m = +1, the vehicle heading angle θ_out is:

[0279] θ_out = atan2(τ_y, τ_x);

[0280] When the reversing segment σ_m = -1, the vehicle's heading angle θ_out is:

[0281] θ_out = atan2(-τ_y, -τ_x);

[0282] Where θ_out is the vehicle heading angle of the output trajectory point; atan2 is the four-quadrant arctangent function; τ_x and τ_y are the two components of the curve tangent vector. In this way, during the reversing segment, the vehicle heading is consistent with the vehicle's front direction, while the trajectory execution direction is opposite to the vehicle heading.

[0283] When there is a direction switching point between adjacent sub-segments, the duplicate location point is deleted, and the target speed is set to 0 at that point, serving as a parking and direction-changing point. Sub-segments before and after the direction-changing point are executed according to their respective direction signs.

[0284] Finally, the straight-ahead fork segment is appended after the endpoint of the main trajectory planning. The trajectory point p_app(q) of the straight-ahead fork segment is generated according to the formula:

[0285] p_app(q) = p_goal + q × h_g, where q belongs to 0 to d_in;

[0286] Where p_app(q) is the point on the straight-ahead fork segment at a distance q from the end point of the main trajectory planning; p_goal is the end point position of the main trajectory planning; h_g is the unit vector of the straight-ahead fork direction; and d_in is the length of the straight-ahead fork segment. The heading angle of the straight-ahead fork segment is always θ_g, and the curvature is 0; when q = d_in, p_app(d_in) = p_target, that is, the end point of the straight-ahead fork segment exactly reaches the target position for pallet pickup and placement.

[0287] This yields a complete planned trajectory from the starting point of the main trajectory to the target position for pallet pickup / pickup. This trajectory is second-order continuous and has constrained curvature within the constant-direction segment. In cases of direction changes, the reversal point is treated as a parking reversal point, satisfying the vehicle's actual execution logic. When the final planned trajectory does not have direction change points, the end-point straight-ahead penalty term constrains the tangential direction of the main trajectory's end to be consistent with the fork-entry direction h_g, and the collinearity of the end-point control points causes the end-point curvature to approach zero. At the connection point, the main trajectory and the straight-ahead fork-entry segment satisfy the conditions of positional continuity, tangential continuity, and curvature continuously approaching zero, achieving a smooth connection. When the end-point curvature verification value is less than the preset zero-reaching threshold ε_κ, the connection is considered to meet the engineering requirements for second-order continuity.

[0288] Figure 6 The scenario shown is an illustration of a situation without reversal. The end of the main trajectory is guaranteed to be a second-order continuous connection with the straight-in fork segment by the endpoint straight-line constraint.

[0289] VI. Implementation and Verification

[0290] To verify the effectiveness of the method of the present invention, a front-fork AGV, a warehouse boundary, a pallet model, and an obstacle model were constructed in a simulation environment. The simulation vehicle parameters were consistent with those in the aforementioned embodiments.

[0291] Scenario 1: The pallet is offset laterally by 0.15 meters, longitudinally by 0.10 meters, and deflected at a 5-degree angle. The lookup table result is State A, meaning no pre-adjustment for straight-line travel is required. After obtaining the initial path solution through a hybrid A* search, the number of direction changes is 0. After pre-optimization and joint optimization using B-splines, the final trajectory has a maximum curvature of 0.58 per meter, which is less than the vehicle's maximum allowable curvature of 0.667 per meter. The end of the main trajectory can smoothly connect with the straight-line fork entry section.

[0292] Scenario 2: The pallet has a lateral offset of 0.25 meters, a longitudinal offset of -0.05 meters, and a deflection angle of -10 degrees. The lookup table result is State B, requiring a 0.60-meter pre-adjustment straight-line driving. After the pre-adjustment, the main trajectory planning is executed. The path has one driving direction switching point, therefore it is divided into two driving direction constant sub-segments. After optimization, the curvature within each sub-segment is continuous, with a maximum curvature of 0.64 per meter, satisfying the curvature constraint; a stop and change direction command is set at the change point.

[0293] Scenario 3: The pallet is offset by 0.30 meters laterally and 0.15 meters longitudinally, with a deflection angle of 15 degrees. This combination is located at the boundary of the lookup table and the corresponding discrete grid point is marked as state C. Therefore, the system directly reports an unplannable anomaly and does not enter the subsequent hybrid A* search and B spline optimization process, thereby avoiding invalid calculations.

[0294] The above verification results show that the present invention can achieve planning feasibility prediction, path search with fewer reversals, curvature-constrained smooth optimization, and end-point straight-fork connection in flexible picking and placing scenarios where pallets have offset and deflection. It is suitable for automatic picking and placing operations of front-fork AGVs in warehousing and logistics scenarios.

[0295] VII. Implementation Method of the Device

[0296] The aforementioned trajectory planning and optimization method can be executed by a trajectory planning device designed for flexible AGV pickup and drop-off scenarios. This device includes a memory and a processor.

[0297] The memory is used to store computer programs and pre-built, programmable parameter space lookup tables.

[0298] When the processor executes the computer program, it implements the following functional modules:

[0299] The prediction module uses the lateral offset, longitudinal offset, and deflection angle of the pallet relative to the AGV as indexes to query the space lookup table of planarable parameters, and determines the starting point of the main trajectory planning based on the query results.

[0300] The endpoint determination module function uses the target position of pallet picking and placing as a reference to trace back the preset fork entry distance in the opposite direction of the straight fork entry direction to obtain the endpoint of the main trajectory planning;

[0301] The search module function searches for initial path solutions using a hybrid A* algorithm based on improved Reeds-Shepp curve cost.

[0302] The pre-optimization module divides the initial path solution into several sub-segments according to the driving direction signs and performs gradient descent pre-optimization with step size normalization factor on each sub-segment.

[0303] The deep optimization module performs cubic B-spline curve fitting on each sub-segment and optimizes the control points with a multi-objective penalty function. For the last sub-segment connecting the straight-forward segment, the multi-objective penalty function also includes an endpoint straight-forward penalty term and sets the three end control points to be collinear and equally spaced along the straight-forward direction unit vectors.

[0304] The verification and splicing module performs curvature upper limit verification and safety distance verification on the optimization results. After the verification is passed, it samples the arc length at equal lengths and splices the straight forward fork segment to output the final trajectory.

[0305] The aforementioned functional modules logically correspond to the processing stages in steps one through three, and their specific algorithm implementations are the same as those in the aforementioned method embodiments, and will not be repeated here. The device can be integrated into the AGV on-board controller, or deployed in an edge computing unit or host computer that communicates with the AGV.

Claims

1. A trajectory planning method for flexible AGV (Automated Guided Vehicle) pickup and drop-off scenarios, characterized in that, Includes the following steps: Step 1: After the AGV arrives at the warehouse location, the sensing module obtains the lateral offset, longitudinal offset, and deflection angle of the pallet relative to the AGV, and queries the pre-built offline planarable parameter space lookup table; when the query result is a planarable state, the current pose of the AGV is used as the starting point for the main trajectory planning. When the query result indicates a plannable state that requires pre-adjustment, first perform pre-adjustment straight-line driving according to the pre-adjustment distance and direction stored in the lookup table, and use the pre-adjusted pose as the starting point for the main trajectory planning. When the query result is unplannable, report the unplannable exception to the upper-level scheduling system and terminate the current planning; based on the target position of pallet picking and placing, backtrack the preset fork entry distance in the opposite direction of the straight fork entry direction to obtain the end point of the main trajectory planning; Step 2: Using the starting and ending points of the main trajectory planning as boundary conditions, a hybrid A* algorithm based on improved Reeds-Shepp curve cost is used for path search. The improved Reeds-Shepp curve cost adds a penalty term proportional to the number of driving direction changes on top of the path geometric length. The searched path is divided into several sub-segments according to the driving direction markers. Gradient descent pre-optimization is performed on the path points of each sub-segment to reduce discrete curvature and improve path smoothness. The pre-optimization applies a step size normalization factor to the gradient of the curvature penalty term. Step 3: For each pre-optimized sub-segment path point, perform arc length parameterization and cubic B-spline curve fitting. For each sub-segment's B-spline control points, perform multi-objective penalty function optimization. The multi-objective penalty function includes a fit preservation term, a curvature penalty term, an obstacle penalty term, and a higher-order smoothness term. For the last sub-segment connecting the straight-forward fork segment, the multi-objective penalty function also includes an endpoint straight-forward penalty term, and sets its three end control points to be equally spaced and collinear along the straight-forward fork direction so that the second-order difference at the end is zero. After optimization, perform curvature upper limit verification and safety distance verification on the output trajectory. After the verification passes, sample with equal arc length and splice the straight-forward fork segments to output the final planned trajectory.

2. The method according to claim 1, characterized in that, The method for constructing the programmable parameter space lookup table includes: In the offline simulation environment, the pallet's lateral offset, longitudinal offset, and deflection angle are discretized and combined for the target AGV model, vehicle kinematics model, fork size, and storage space constraints. For each discrete combination, planning tests are performed with and without pre-adjustment straight-line driving enabled. Based on the test results, the corresponding combination is marked as a first state, a second state, or a third state. The first state indicates successful planning without pre-adjustment straight-line driving; the second state indicates successful planning only after pre-adjustment straight-line driving; and the third state indicates unsuccessful planning even after pre-adjustment straight-line driving. The lookup table stores the recommended pre-adjustment distance and recommended pre-adjustment driving direction corresponding to the second state. When the query result is the second state, the pre-adjustment driving direction flag is positive (1) indicating forward movement along the AGV's current heading, and negative (1) indicating reverse movement along the AGV's current heading. When the query result is the third state, the reported content includes the current pallet deviation parameters and an unplannable state flag.

3. The method according to claim 2, characterized in that, In the discretized combination traversal: the horizontal offset ranges from -0.30 meters to 0.30 meters, with a distance from the step size not exceeding 0.05 meters; the vertical offset ranges from -0.20 meters to 0.20 meters, with a distance from the step size not exceeding 0.05 meters; and the deflection angle ranges from -15 degrees to 15 degrees, with a distance from the step size not exceeding 2.5 degrees.

4. The method according to claim 1, characterized in that, The preset fork entry distance d_in is determined by the formula d_in=L_fork+d_clear+d_ref, where L_fork is the fork extension length, d_clear is the safety clearance between the fork tip and the pallet, and d_ref is the longitudinal distance from the AGV planning reference point to the fork root. The coordinates p_goal of the main trajectory planning endpoint are determined by the formula p_goal=p_target-d_in×h_g, where p_target is the target position coordinates for pallet pickup and drop, h_g=(cosθ_g,sinθ_g) is the unit vector of the straight fork entry direction, and θ_g is the vehicle heading angle corresponding to the straight fork entry direction.

5. The method according to claim 1, characterized in that, The improved Reeds-Shepp curve cost function is calculated as follows: For a candidate Reeds-Shepp curve, after removing motion primitive segments of zero length, M_rs non-zero motion primitive segments are obtained, each with a sign length l_q; the total geometric length is calculated using the formula L=sum|l_q|; the number of direction switching is calculated using the formula n=sum I(sign(l_q)≠sign(l_{q-1})); the corrected cost is calculated using the formula C_RS=L×(1+w_dir×n), where w_dir is a dimensionless direction switching penalty weight greater than 0, and I(·) is an indicator function; the value of the direction switching penalty weight w_dir ranges from 0.5 to 5.0; the heuristic cost h of the hybrid A* search is the shortest Reeds-Shepp geometric length from the current node to the target node and does not include the direction switching penalty to ensure that the heuristic value does not overestimate the true optimal cost.

6. The method according to claim 1, characterized in that, The method for dividing segments according to driving direction signs is as follows: During the hybrid A* search process, the driving direction marker σ is retained for each path sampling point or each motion primitive segment, where σ=+1 indicates forward movement and σ=-1 indicates reverse movement. Adjacent driving direction markers are checked sequentially along the path execution order. If the driving direction markers of adjacent motion primitive segments are different, their common connection point is marked as a driving direction switching point. The path between two adjacent driving direction switching points constitutes a driving direction constant sub-segment. When the search result does not save the driving direction marker, the driving direction marker of the j-th sampling point is estimated using the formula σ_j=sign((p_j-p_{j-1})·h_j), where h_j=(cosθ_j,sinθ_j) is the vehicle heading unit vector at the j-th sampling point. When the absolute value of the inner product is less than the preset threshold ε_σ, σ_j retains the marker of the previous sampling point.

7. The method according to claim 1, characterized in that, The pre-optimization process includes discrete curvature calculation, step size normalization, and five-point difference smoothing; for the j-th intermediate path point in the sub-segment, the turning angle, discrete curvature, and step size normalization factor are calculated according to the following formulas: φ_j=atan2(a_x_j×b_y_j-a_y_j×b_x_j,a_x_j×b_x_j+a_y_j×b_y_j); κ_j=|φ_j| / (0.5×(La_j+Lb_j)+ε_s); η_j=max(La_j,Lb_j)^2 / (La_j×Lb_j+ε_s^2); Where a_j=p_j-p_{j-1} is the displacement vector of the previous segment, b_j=p_{j+1}-p_j is the displacement vector of the next segment, La_j and Lb_j are the lengths of the previous and next segments respectively, and ε_s is a positive threshold to prevent division by zero; when κ_j exceeds the maximum allowable curvature κ_max of the vehicle, a secondary penalty is applied to the excess part, and the gradient corresponding to the curvature penalty term is divided by η_j and used for path point update; the geometric smoothing term adopts the five-point difference form D_j=p_{j-2}-4×p_{j-1}+6×p_j-4×p_{j+1}+p_{j+2}, and ||D_j||^2 is accumulated for the intermediate path points with index j∈[3,N_m-3] to obtain J_smooth_pre.

8. The method according to claim 1, characterized in that, The cubic B-spline curve fitting and multi-objective penalty function optimization include: calculating the cumulative arc length of the path points of the sub-segment and normalizing it to the parameter u_j∈[0,1]; constructing a cubic B-spline basis function using uniform node vectors with repeated endpoints; using the start and end points of the sub-segment as the first and last control point constraints, respectively; setting the end tangential control points according to the headings of the start and end points of the sub-segment; and solving for the initial control points using the constrained least squares method with regularization terms. The total objective function of each sub-segment is calculated according to the following formula: J_total=w1×J_stay+w2×J_curv+w3×J_obs+w4×J_smooth_bsp+w5×J_end; Where J_stay is the fit preservation term, J_curv is the curvature penalty term, J_obs is the obstacle penalty term, and J_smooth_bsp is the higher-order smoothness term based on the third-order difference of the four adjacent control points; the endpoint straight-line constraint term w5×J_end only applies to the last segment connecting the straight-line fork, and w5 is zero for the remaining segments; the endpoint straight-line constraint term is further decomposed into J_end=||t_end-h_g||^2+γ_end×κ_end^2, where t_end is the unit tangent at the end of the main trajectory, κ_end is the curvature at the end of the main trajectory, and γ_end is the sub-weight of the curvature at the end; w1, w2, w3, w4 and γ_end are all weight coefficients greater than 0, and w5 is a weight coefficient greater than 0 for the last segment connecting the straight-line fork; the L-BFGS quasi-Newton optimization method is used to iteratively solve the control point coordinates.

9. The method according to claim 1, characterized in that, The curvature upper limit check is performed by traversing all sampling points of the output trajectory to verify that |κ_j|≤κ_max; the safe distance check is performed by traversing all sampling points to verify that the distance to the nearest obstacle dist_j≥d_safe; when any check fails, the corresponding penalty weight is increased and the multi-objective penalty function optimization is re-executed; a merged retry upper limit counter N_retry is set, and when the cumulative number of retries reaches N_retry and still fails, a planning failure exception is reported to the upper-level scheduling system; The value of N_retry ranges from 2 to 5 times, and the value of d_safe ranges from 0.10 meters to 0.50 meters.

10. A trajectory planning device for flexible AGV picking and placing scenarios, used to execute the method according to any one of claims 1-9, characterized in that, It includes a memory and a processor, the memory storing a computer program, and the processor executing the computer program to implement: The prediction module uses the lateral offset, longitudinal offset, and deflection angle of the pallet relative to the AGV as indexes to query a pre-built offline planarable parameter space lookup table, and determines the starting point of the main trajectory planning based on the query results. The endpoint determination module function uses the target position of pallet picking and placing as a reference to trace back the preset fork entry distance in the opposite direction of the straight fork entry direction to obtain the endpoint of the main trajectory planning; The search module function searches for initial path solutions using a hybrid A* algorithm based on an improved Reeds-Shepp curve cost, which includes a penalty term proportional to the number of driving direction changes. The pre-optimization module divides the initial path solution into several sub-segments according to the driving direction signs and performs gradient descent pre-optimization with step size normalization factor on each sub-segment. The deep optimization module performs cubic B-spline curve fitting on each sub-segment and optimizes the control points with a multi-objective penalty function that includes a fit preservation term, a curvature penalty term, an obstacle penalty term, and a higher-order smoothness term. For the last sub-segment connecting the straight fork segment, the multi-objective penalty function also includes an endpoint straight penalty term and sets its three end control points to be collinear and equally spaced along the straight fork direction unit vectors. The verification and splicing module performs curvature upper limit verification and safety distance verification on the optimization results. After the verification is passed, it samples the arc length at equal lengths and splices the straight forward fork segment to output the final trajectory.

Images