A trajectory planning method for rail and non-rail mixed areas

Abstract

The present application belongs to the technical field of trajectory planning, and particularly relates to a trajectory planning method for a track and non-track mixed area, which dynamically allocates a path point sequence for an automatic guided vehicle, and switches a control mode according to a region type where a current target point is located: a first control instruction of path tracking is generated in a track region; in a non-track region, a smooth trajectory is planned with a pose and a curvature of a previous track region exit as initial reference quantities, and a second control instruction is generated; by constraint shaping on the first control instruction and adjustment on the second control instruction, it is ensured that an end point state of path tracking and a start point state of trajectory planning at a connection point of the two regions satisfy a continuity rule of position, heading and curvature, and finally the instructions are respectively sent to corresponding control loops for execution; the present application realizes smooth and coherent navigation control of the automatic guided vehicle between a fixed track and a free travel region.

Description

Technical Field

[0001] This invention belongs to the field of trajectory planning technology, and in particular relates to a trajectory planning method for mixed track and non-track areas. Background Technology

[0002] In automated warehousing and logistics systems, mobile robots such as automated guided vehicles (AGVs) typically need to alternate between fixed guide track areas and non-track autonomous navigation areas. The current common approach is to use pure path tracking control in the fixed track section and an independent global motion planning algorithm in the free navigation section. When a robot switches from one area to another, the control system often directly switches the underlying command source. This direct switching method, due to the inherent differences in target generation logic and kinematic constraint handling between the two types of control algorithms, easily causes discontinuous jumps in the position, heading, and curvature of the synthesized trajectory at the switching boundary. This kinematic discontinuity leads to significant robot jitter, decreased positioning accuracy, and in severe cases, even collision risks, compromising the smoothness of the workflow and the overall system safety. Furthermore, the high-order trajectory optimization methods used to achieve smooth transitions typically involve solving complex nonlinear problems, incurring heavy computational burdens and making it difficult to meet real-time requirements in embedded vehicle controllers with limited computing resources. Therefore, designing a lightweight trajectory planning method that, under the constraints of robot dynamics and controller response capabilities, ensures that the synthesized trajectory meets strict continuity requirements in position, heading, and rate of curvature change when switching between mixed track and non-track areas is a pressing technical problem that needs to be solved. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention proposes a trajectory planning method for mixed track and non-track areas. This method dynamically allocates a sequence of waypoints to the automated guided vehicle (AGV) and switches control modes based on the type of area where the current target point is located: In track areas, a first control command for path tracking is generated; in non-track areas, a smooth trajectory is planned using the pose and curvature of the previous track area exit as initial reference values, generating a second control command. By constraining and shaping the first control command and adjusting the second control command, it is ensured that at the junction of the two areas, the endpoint state of path tracking and the starting state of trajectory planning satisfy the continuity rules of position, heading, and curvature. Finally, the commands are sent to the corresponding control loops for execution. This invention achieves smooth and continuous navigation control of the AGV between fixed tracks and free-roaming areas.

[0004] To achieve the above objectives, the present invention provides the following technical solution:

[0005] A trajectory planning method for mixed track and non-track areas includes:

[0006] Based on the real-time status and task information of the automated guided vehicles (AGVs), a path point sequence consisting of continuous spatial location points is dynamically assigned to each AGV.

[0007] For the current target point in the path point sequence, determine its region type: if it is located in a preset track region, generate a first control command for path tracking; if it is located in a preset non-track region, use the pose and curvature at the exit of the adjacent previous track region as the initial reference for trajectory planning, and start the autonomous trajectory planning process.

[0008] Using the initial reference value as the starting boundary condition, a smooth non-track segment trajectory is obtained to reach the current target point, and a second control command for trajectory tracking is generated;

[0009] The first control command is constrained and shaped to obtain a constrained and shaped first control command, so that at the junction of the track area and the non-track area, the path tracking endpoint state achieved by the constrained and shaped first control command and the starting state of the non-track segment smooth trajectory planned by the second control command satisfy the preset continuity rule.

[0010] The first control command after constraint shaping is output to the path tracking control loop for the track area, and the second control command is output to the autonomous trajectory tracking control loop for the non-track area.

[0011] Specifically, the continuity rule includes at least position, heading angle and curvature continuity, and the constraint shaping includes limiting the turning rate and acceleration;

[0012] The dynamic allocation of a path point sequence consisting of continuous spatial location points for each automated guided vehicle includes:

[0013] Obtain a comprehensive dataset containing the real-time location coordinates of each automated guided vehicle, its current cargo status, the coordinates of the task target point, and the task priority;

[0014] Based on the task target point coordinates and real-time location coordinates in the comprehensive dataset, an initial shortest path is calculated for each automated guided vehicle (AGV) from its real-time location to its task target point in a predefined warehouse environment grid map; the initial shortest path consists of a series of sequentially connected map grid center points.

[0015] Specifically, dynamically assigning a waypoint sequence consisting of continuous spatial location points to each automated guided vehicle also includes:

[0016] Based on the initial shortest path calculated for all automated guided vehicles (AGVs), and the preset dimensions and speed model of each AGV, the process of each AGV traveling along its initial shortest path is simulated to obtain the probability of spatial overlap or path intersection between any two vehicles within a preset time window.

[0017] Based on the initial shortest path and the conflict probability, the initial shortest path with conflict is corrected to obtain a preliminary path point sequence without conflict for each automated guided vehicle.

[0018] Based on the conflict-free initial path point sequence and the kinematic constraint of the minimum turning radius of the automated guided vehicle, the turning points in the conflict-free initial path point sequence are smoothed to generate a smooth path with continuous curvature, and the smooth path is discretized into a smooth path point sequence composed of continuous spatial position points.

[0019] Specifically, dynamically assigning a waypoint sequence consisting of continuous spatial location points to each automated guided vehicle also includes:

[0020] Continuously acquire task update signals or fault signals from the scheduling system or the vehicle's underlying system;

[0021] Identify the first set of automated guided vehicles whose paths need to be adjusted due to the task update signal or fault signal;

[0022] Based on the current task priority of each automated guided vehicle in the first automated guided vehicle set, an execution order queue for path resource reallocation is generated.

[0023] Based on the execution order queue, and using the latest task target point and real-time position of each automated guided vehicle in the first automated guided vehicle set, a first alternative smooth path point sequence is generated for each automated guided vehicle.

[0024] Detect spatial positional conflicts between the first candidate smooth path point sequence of each automated guided vehicle and the first candidate smooth path point sequence corresponding to the remaining automated guided vehicles, and generate a set of conflict-free first smooth path point sequences by combining a preset conflict resolution strategy.

[0025] Based on the set of conflict-free first smooth path point sequences, the tag information of the corresponding automated guided vehicle is embedded into the corresponding automated guided vehicle.

[0026] Specifically, the first control instructions for path tracking are generated, including:

[0027] Based on the current real-time position coordinates and heading angle of the automated guided vehicle, and the coordinates of the preset track reference path points in the path point sequence, the lateral position deviation and heading angle deviation values ​​of the current real-time position of the automated guided vehicle relative to the preset track path are obtained.

[0028] Based on the calculated lateral position deviation and heading angle deviation values, the initial steering control quantity used to correct the deviation is calculated.

[0029] Specifically, generating the first control command for path tracking also includes:

[0030] Based on the reference speed value defined in the path point sequence, and combined with the absolute values ​​of the lateral position deviation value and the heading angle deviation value, the reference speed value is dynamically corrected to obtain the final speed control command.

[0031] Based on the physical constraints of the maximum steering angular velocity and maximum acceleration of the automated guided vehicle actuator, the initial steering control quantity and the speed control command are constrained to obtain constrained steering control commands and speed control commands.

[0032] The constrained steering control command and speed control command are encapsulated and combined to generate the first control command for path tracking.

[0033] Specifically, a second control command for trajectory tracking is generated, including:

[0034] Using the pose and curvature recorded at the exit of the adjacent preceding track region as initial references, and the coordinates of the current target point in the non-track region, a parametric smooth trajectory connecting the start and end points is generated.

[0035] Based on the parameterized smooth trajectory, the feedforward control quantity for trajectory tracking is calculated using the trajectory parameter differentiation method to obtain the feedforward speed command;

[0036] Based on the current real-time position and heading angle of the automated guided vehicle, as well as the coordinates and tangent direction of the projection point closest to the current real-time position on the parameterized smooth trajectory, the lateral position error and heading error of the trajectory tracking are obtained.

[0037] Specifically, generating the second control command for trajectory tracking also includes:

[0038] Based on the lateral position error and heading error, the optimal state feedback gain matrix for eliminating the error is calculated.

[0039] The lateral position error and heading error are multiplied by the gain value corresponding to the optimal state feedback gain matrix to obtain the feedback steering angle adjustment.

[0040] The feedforward steering angle command is superimposed with the feedback steering angle adjustment to obtain the synthetic steering control command;

[0041] Based on the maximum steering angle rate constraint of the automated guided vehicle actuator, the synthesized steering control command is constrained to obtain the constrained steering control command.

[0042] The feedforward speed command is smoothed and its rate of change is limited to generate a constrained speed control command.

[0043] The constrained steering control command and the constrained speed control command are encapsulated to generate a second control command for trajectory tracking in non-track areas.

[0044] Specifically, the path tracking endpoint state achieved by the first control command after constraint shaping, and the starting state of the non-track segment smooth trajectory planned by the second control command, satisfy a preset continuity rule, including:

[0045] Based on the path point sequence and predefined region type attributes, the connection point is obtained, specifically by: sequentially traversing each path point in the path point sequence and querying the region type attribute corresponding to each path point based on the predefined warehouse environment grid map; marking the first path point whose region type attribute is a non-track region, and determining and recording the preceding path point in the path point sequence as the connection point.

[0046] Based on the path point sequence, obtain the initial reference value at the connection point;

[0047] Based on the maximum steering angle rate constraint of the automated guided vehicle actuator, constraint shaping is performed on the steering control command in the first control command used for path tracking.

[0048] Specifically, the path tracking endpoint state achieved by the first control command after constraint shaping, and the starting state of the non-track segment smooth trajectory planned by the second control command, satisfy a preset continuity rule, including:

[0049] Based on the maximum acceleration constraint of the automated guided vehicle actuator, constraint shaping is performed on the speed control command in the first control command used for path tracking;

[0050] The constrained steering control command and the constrained speed control command are combined to obtain the constrained first control command;

[0051] The actual state of the automated guided vehicle at the connection point is obtained, including the actual position, actual heading angle and actual curvature.

[0052] Determine whether the actual state and the initial reference value satisfy a preset continuity rule; the continuity rule requires that the distance error between the actual position and the position in the initial reference value is less than a first error threshold, the difference between the actual heading angle and the heading angle in the initial reference value is less than a second error threshold, and the difference between the actual curvature and the curvature in the initial reference value is less than a third error threshold.

[0053] If the continuity rule is satisfied, the path tracking endpoint state implemented by the first control command after constraint shaping is determined to be continuous with the starting state of the non-track segment smooth trajectory planned by the second control command.

[0054] Compared with the prior art, the beneficial effects of the present invention are:

[0055] This invention addresses the shortcomings of existing technologies by dynamically allocating continuous path point sequences based on the real-time status and task of the automated guided vehicle (AGV), and switching the corresponding control mode according to the type of track or non-track region where the current target point is located. This effectively solves the core problem of trajectory continuity in hybrid navigation environments. In particular, during non-track trajectory planning, the previously verified exit pose and curvature of the track region are forcibly used as initial boundary conditions, ensuring strict continuity of position and heading at switching points in principle. Furthermore, by applying constraints conforming to the dynamic limits of the track segment tracking control commands and transferring these constraints to the non-track trajectory generation process, the synthesized trajectory also meets the continuity and executability requirements in terms of curvature change rate, thus avoiding control instability caused by abrupt changes in motion state. This lightweight strategy replaces complex global trajectory reconstruction with locally constrained planning at key connection points, significantly reducing computational complexity while strictly satisfying kinematic continuity constraints, and achieving a natural, stable, and real-time transition for the AGV from high-precision path tracking to autonomous smooth trajectory tracking. Attached Figure Description

[0056] Figure 1 This is a flowchart of a trajectory planning method for a mixed track and non-track area according to the present invention;

[0057] Figure 2 This is a logic diagram of a trajectory planning method for mixed track and non-track areas according to the present invention. Detailed Implementation

[0058] To facilitate understanding of the background and application scope of this application, the following describes specific implementation methods in conjunction with typical warehousing and logistics automation scenarios.

[0059] In material handling tasks in large e-commerce warehouses or smart manufacturing plants, the operating environment and tasks of Automated Guided Vehicles (AGVs) present a combination of interwoven track and non-track areas, and high requirements for dynamism and real-time performance. Within the same storage unit, there are fixed guide tracks for high-speed, stable transportation, as well as vast rack storage areas and loading / unloading platforms requiring flexible access to goods—areas with free navigation. Task demands are constantly changing dynamically, such as order waves and production line material requests. Path resources are subject to multiple constraints, such as narrow aisles between racks, single-line rules, and avoidance of manual work areas. Furthermore, multiple AGVs need to work in parallel and collaboratively to ensure overall throughput efficiency. When an AGV moves from a highly structured track area into an unstructured free area, its motion control mode needs to be switched. Improper switching can easily lead to abrupt changes in motion state at the boundary.

[0060] Existing methods generally employ two approaches: First, a "switching" control strategy is used, employing a tracking controller based on path deviation feedback in the track area and switching to a trajectory planning and tracking controller based on a global map in the non-track area, directly switching the control command source upon reaching the boundary. Second, a "globally unified" planning strategy is adopted, ignoring the region type and calculating a unified, high-order, smooth reference trajectory for the entire journey from the beginning, such as using spline curves or optimization algorithms, and using a single trajectory tracking controller throughout the entire process.

[0061] When facing area switching, the former type of controller often experiences instantaneous changes in vehicle position, heading, and even speed at the switching point due to inherent differences in target generation logic, error definition, and handling of vehicle dynamics constraints. This leads to vehicle vibration, stopping adjustments, or positioning drift, disrupting the smoothness of the workflow and creating safety hazards. While the latter type of controller can theoretically guarantee a smooth trajectory throughout, its complex global trajectory optimization algorithm becomes computationally burdensome when faced with dynamically inserted new tasks, temporary lane occupancy by other vehicles, or local area malfunctions requiring real-time replanning. It struggles to balance the computing power of the onboard embedded controller with real-time requirements, often resulting in replanning delays or sluggish responses.

[0062] The common dilemma of both control methods is that neither incorporates the crucial physical constraint of the continuity of the motion state of the "region switching point" as a core factor into the path allocation and control command generation logic. This results in either passively bearing the switching impact during later execution or incurring an unbearable computational cost in pursuit of global smoothness.

[0063] The core processing logic of this application is based on the aforementioned common dilemma. It does not rely on the traditional paradigm of "planning first, then switching" or "global optimization regardless of cost." Instead, it first identifies the connection point between the track and non-track areas as the key to determining the smoothness of the entire operation process, and establishes the strict continuity of the position, heading, and curvature motion state at this point as an uncompromising prerequisite that the planning and control algorithms must satisfy. To this end, when dynamically allocating the path point sequence for the automated guided vehicle, the connection point from the track area to the non-track area is identified simultaneously, and the initial reference value of the point is accurately calculated.

[0064] This boundary condition triggers a collaborative design across control modes. In the track region, when generating the first control command, not only is path tracking accuracy prioritized, but also a constraint shaping algorithm based on the physical limits of the vehicle's actuators, including maximum steering angular rate and maximum acceleration, is used to ensure that the vehicle's actual motion state stably and accurately matches the initial reference value when arriving at the connection point. In the off-track region, when generating the second control command, the initial reference value is forcibly used as the starting boundary condition for trajectory planning, thereby ensuring that the planned smooth trajectory of the off-track segment seamlessly connects with the end state of the track segment at the starting point.

[0065] Unlike conventional methods, this embodiment places the connection point state calculation, track segment command constraint shaping, and non-track segment trajectory boundary constraints within a closed-loop feedforward-feedback architecture. Each time the vehicle approaches the switching area, the control commands are fine-tuned in real time based on the current state, and continuity rules are verified to ensure a smooth final switch. This avoids large-scale state corrections later and circumvents complex global replanning.

[0066] It should be emphasized that the above description is not an exhaustive list of algorithmic details, but rather clarifies the technical starting point and reproducible implementation boundaries of this embodiment:

[0067] Given a known warehouse environment map and dynamic task flow, distinguishable area types, and quantifiable vehicle kinematics and dynamics constraints, this approach identifies connection points and calculates initial reference values. These values ​​then drive the constraint shaping of track segment control commands and the boundary condition planning of non-track segment trajectories, enabling strictly continuous and stable switching of the automated guided vehicle's (AGV) motion state between mixed areas. This approach transforms the root cause of state discontinuity during switching into a prerequisite cooperative condition that must be met in the design of planning and control algorithms. In principle, it resolves the triple challenges of existing technologies in terms of operational smoothness, real-time replanning capability, and system safety, providing a unified and robust control framework for efficient and flexible material handling in automated warehousing and logistics systems.

[0068] Please see Figure 1 and Figure 2The present invention provides an embodiment of a trajectory planning method for a mixed track and non-track area, comprising the following steps:

[0069] S1. Based on the real-time status and task information of the automated guided vehicles (AGVs), dynamically allocate a path point sequence consisting of continuous spatial location points to each AGV.

[0070] It should be further explained that in this embodiment, each automated guided vehicle (AGV) is dynamically assigned a path point sequence consisting of continuous spatial location points, including:

[0071] S101. Obtain a comprehensive dataset containing the real-time location coordinates, current cargo status, task target point coordinates, and task priority of each automated guided vehicle (AGV). It should be further explained that the comprehensive dataset in this embodiment is collected in the following ways: real-time location coordinates are obtained through a fusion positioning system between the vehicle-mounted global satellite navigation system module and the odometer; the current cargo status is calculated by measuring the total weight using pressure sensors installed on the cargo platform and combining this with the vehicle's empty weight; the task target point coordinates and task priority are obtained in real-time from task instructions issued by the central dispatch system via a wireless communication module. These data, after being timestamped, together constitute the comprehensive dataset used for route decision-making.

[0072] S102. Based on the task target point coordinates and real-time location coordinates in the comprehensive dataset, calculate an initial shortest path from the real-time location of each automated guided vehicle to its task target point in a predefined warehouse environment grid map; the initial shortest path consists of a series of sequentially connected map grid center points.

[0073] It should be further explained that, in this embodiment, calculating an initial shortest path from the real-time position of each automated guided vehicle to its mission target point includes:

[0074] A101. Based on the two-dimensional array data of the predefined warehouse environment raster map, the system adopts a row-by-row and column-by-column traversal and conditional judgment rules to identify and mark all raster cells with passable attribute values; each passable raster cell is defined as a graph node, and each graph node is assigned a unique integer index as an identifier.

[0075] A102. Based on the row and column numbers of the grid cells corresponding to each graph node in the two-dimensional array, a four-neighbor or eight-neighbor adjacency relationship determination algorithm is used to determine the adjacent passable graph nodes that exist for each graph node in the horizontal and vertical directions or simultaneously in the diagonal direction; for each current graph node, the connection between it and each adjacent passable graph node is defined as an undirected edge.

[0076] A103. Based on predefined passage cost rules, assign a weight value representing the movement cost to each defined edge. It should be further explained that the specific calculation steps for assigning weight values ​​to edges in this embodiment are as follows: Based on a predefined terrain attribute mapping table, obtain the terrain attribute codes of the two grid cells connected to the edge; Based on a preset cost calculation formula, which is typically a function weighted according to the difference in terrain attributes, such as multiplying the slope difference by a first coefficient and the road surface type difference level by a second coefficient, and then summing the results; Assign the calculated cost sum as the dynamic weight value of the edge. If the two grid cells have the same attributes, directly assign a preset unified basic weight value.

[0077] A104. Based on all defined graph nodes, edges, and their corresponding weights, generate and store a weighted graph model for path search using an adjacency list or adjacency matrix data structure. The adjacency list data structure maintains a list for each graph node, recording all its adjacent graph nodes and the weights of their corresponding edges. The adjacency matrix data structure uses a two-dimensional array, where the row and column indices correspond to graph nodes, and the array element values ​​represent the weights of the edges between two corresponding graph nodes, with infinity being used if there is no connection.

[0078] A105. Based on the real-time location coordinates in the comprehensive dataset, the Euclidean distance nearest neighbor search algorithm is used to calculate the Euclidean distance between the grid center point coordinates of each graph node and the real-time location coordinates in all graph nodes of the weighted graph model; the graph node with the smallest Euclidean distance is selected and determined as the starting graph node.

[0079] A106. Based on the coordinates of the task target point in the comprehensive dataset, using the same Euclidean distance nearest neighbor search algorithm, calculate the Euclidean distance between the grid center point coordinates of each graph node and the coordinates of the task target point in all graph nodes of the weighted graph model; select and determine the graph node with the smallest Euclidean distance as the target graph node.

[0080] A107. Create and initialize a distance label and a predecessor node label for each graph node in the weighted graph model; initialize the distance label values ​​of all graph nodes to a very large value representing infinity, and initialize the predecessor node labels of all graph nodes to null values; set the distance label value of the starting graph node to zero; create a priority queue based on a min-heap data structure, and add the starting graph node and its distance label value to the priority queue.

[0081] A108. Repeat the following operations until the priority queue is empty: Take out and remove the graph node with the smallest current distance label value from the priority queue as the current node; determine whether the current node is the target graph node. If so, terminate the iteration process; otherwise, traverse all adjacent graph nodes of the current node in the weighted graph model; for each adjacent graph node, calculate the temporary distance from the starting graph node to the adjacent graph node via the current node. The temporary distance is equal to the distance label value of the current node plus the weight value of the edge connecting the current node and the adjacent graph node; compare the temporary distance with the distance label value currently recorded by the adjacent graph node; if the temporary distance is smaller, update the distance label value of the adjacent graph node to the temporary distance, update the label of its predecessor node to the current node, and add or update the position of the adjacent graph node and its updated distance label value in the priority queue.

[0082] A109. Execute the path backtracking step. After the algorithm iteration terminates, starting from the target graph node, trace back forward according to the predecessor node label recorded by each graph node until the starting graph node with an empty predecessor node label is reached, thereby obtaining a shortest path node sequence composed of ordered graph nodes from the starting graph node to the target graph node.

[0083] A110. Traverse each graph node in the shortest path node sequence. Based on the modeling rules of the weighted graph model, find and obtain the two-dimensional coordinates of the grid center point corresponding to each graph node through the mapping relationship between the graph node index and the grid row and column coordinates.

[0084] A111. According to the arrangement order of the graph nodes in the shortest path node sequence, connect all the obtained grid center point coordinates in sequence to generate and output an initial shortest path composed of a series of map grid center point coordinates.

[0085] S103. Based on the initial shortest path calculated for all automated guided vehicles, and the preset external dimensions and speed model of each automated guided vehicle, simulate the process of each automated guided vehicle traveling along its initial shortest path, and obtain the probability of spatial position overlap or path intersection between any two vehicles within a preset time window.

[0086] It should be further explained that the probability of spatial overlap or path intersection between any two vehicles within a preset time window in this embodiment includes:

[0087] B101. Based on the initial shortest path and the preset cruising speed in the speed model, predict the position sequence of each automated guided vehicle; specifically including:

[0088] Based on the preset total duration of the future prediction time window and the fixed simulation time step, a series of discrete future time points are generated using the equal-interval discretization method.

[0089] For each automated guided vehicle, based on its preset cruising speed, a uniform linear motion model is used to calculate the theoretical straight-line distance it has traveled along the initial shortest path at each future time point, which is then used as the cumulative distance traveled.

[0090] Based on the accumulated travel distance, a piecewise linear interpolation algorithm is used to determine the path segment where the current travel distance is located in the path point sequence of the initial shortest path. Based on the coordinates of the two ends of the path segment and the proportion of the accumulated travel distance on the segment, the estimated two-dimensional coordinates of the automated guided vehicle at the current time point are calculated and obtained.

[0091] For each automated guided vehicle, a time-position sequence consisting of a series of "time point-estimated coordinate" pairs is generated.

[0092] B102. Based on the preset external dimensions of each automated guided vehicle (AGV) and the time-position sequence, an axis-aligned bounding box generation algorithm is used to generate a rectangular boundary region for each AGV at each discrete time point; specifically including:

[0093] Based on the time-location sequence, the estimated two-dimensional coordinates of an automated guided vehicle at a specific time point are obtained, which are used as the center point of the vehicle at that moment.

[0094] Based on the vehicle length and width in the preset external dimensions of the automated guided vehicle, a rectangular region construction method is adopted, with the center point as the geometric center, to construct a rectangle whose long side is parallel to the X-axis of the global coordinate system and whose short side is parallel to the Y-axis.

[0095] The coordinates of the four vertices of the rectangular region are calculated from the coordinates of the center point, half the length of the vehicle body, and half the width of the vehicle body. This region is defined as the axis-aligned bounding box of the vehicle at the current time step.

[0096] B103. Based on the axis-aligned bounding boxes generated by all automated guided vehicles at all discrete time points, a split axis theorem detection algorithm is used to perform collision detection on a time-by-time and vehicle-by-vehicle basis; specifically including:

[0097] For each discrete time point, iterate through all pairs of automated guided vehicles in the system.

[0098] For any two automated guided vehicles (AGVs) with their axis aligned bounding boxes at the current time point, the projection interval calculation method is used to calculate the projection intervals of the two rectangles on the X-axis and Y-axis of the global coordinate system, respectively. The projection interval of each rectangle is composed of the minimum and maximum coordinate values ​​of all its vertices on that coordinate axis.

[0099] An interval overlap determination logic is adopted to determine whether the projection intervals of two rectangles on the X-axis overlap, and to determine whether the projection intervals on the Y-axis overlap.

[0100] If it is determined that the projection intervals of the two rectangles on the X-axis and Y-axis overlap, then it is determined that the two automated guided vehicles have a spatial position conflict at the current time point.

[0101] B104. Based on the judgment result of the separation axis theorem detection algorithm, a conflict event structured recording method is used to create a data record for each detected spatial location conflict; specifically including:

[0102] For each newly detected spatial location conflict, assign a globally unique event identifier.

[0103] This record contains the unique device identifiers of the automated guided vehicles (AGVs) on both sides of the conflict and the index of the discrete time point where the conflict occurred.

[0104] Based on the vertex coordinates of the two axis-aligned bounding boxes that are in conflict, the geometric center calculation method is used to calculate the coordinates of the center point of the overlapping area of ​​the two rectangles, and these coordinates are recorded as the conflict position coordinates.

[0105] All generated conflict event records are temporarily stored in a temporary data buffer.

[0106] For each detected spatial location conflict, a frequency estimation algorithm based on historical data statistics is used to calculate the probability of the conflict actually occurring within a future time window; the frequency estimation algorithm is calculated based on the ratio of the number of historical conflicts occurring at the same location and with the same vehicle combination to the total number of predicted conflicts.

[0107] Add a conflict probability field to the conflict event record to store the calculated probability value.

[0108] B105. Based on all conflict event records temporarily stored in the temporary data buffer, generate and output the final conflict prediction report; specifically including:

[0109] An ascending sorting algorithm based on time-point indexes is used to sort all conflict event records.

[0110] For the sorted list of records, an adjacent record merging and checking method is used to merge duplicate conflict records describing the same vehicle pair that occurred at the same time point or within a very short adjacent time.

[0111] The processed list of records is encapsulated into a data table with a fixed field structure to generate a structured conflict prediction report. Each row of data in the report corresponds to a conflict event, including the event identifier, the identifiers of the two conflicting vehicles, the conflict time index, the conflict location X coordinate, and the conflict location Y coordinate.

[0112] S104. Based on the initial shortest path and the conflict probability, the initial shortest path with conflicts is corrected to obtain a preliminary path point sequence without conflicts for each automated guided vehicle; it should be further noted that the correction of the initial shortest path with conflicts in this embodiment includes:

[0113] S1041. Extract the recorded conflict time step, conflict location coordinates, and conflict probability. Based on the speed model of each automated guided vehicle, backtrack to calculate the start and end points of the conflict on its respective initial shortest path. This determines the conflict path segment that needs to be adjusted for each vehicle with a conflict and records the conflict probability corresponding to that path segment.

[0114] S1042. Based on the determined conflict path segments, combined with A The path replanning algorithm takes the starting point of the conflict path segment as the planning start point and the ending point as the planning target, and marks the conflict area on the map as a temporary obstacle area with dynamic passage cost according to the conflict probability. It calculates and generates one or more alternative paths to bypass the conflict area for each affected vehicle; wherein, the passage cost of the temporary obstacle area is positively correlated with the conflict probability.

[0115] S1043. Based on the preset path evaluation cost function, perform a comprehensive performance evaluation on each generated candidate path; the path evaluation cost function is calculated by weighted summation, and its variables include at least the total length of the candidate path, the number of times the direction changes in the path, the minimum distance between each point on the path and the boundary of the conflict area, and the comprehensive conflict probability of the path crossing the conflict area.

[0116] S1044. Based on the calculated comprehensive cost value of each alternative path, select the path with the smallest comprehensive cost value from all alternative paths for each conflicting automated guided vehicle and determine it as the corrected path segment for that vehicle.

[0117] S1045. Based on the corrected path segments of all vehicles and their original path segments that do not conflict, the corrected path segments are seamlessly replaced with the original conflicting path segments by using path sequence splicing, and the connection points are smoothed to generate and output a continuous and conflict-free preliminary path point sequence for each automated guided vehicle.

[0118] S105. Based on the conflict-free preliminary path point sequence and the kinematic constraint of the minimum turning radius of the automated guided vehicle, the turning points in the conflict-free preliminary path point sequence are smoothed to generate a smooth path with continuous curvature, and the smooth path is discretized into a smooth path point sequence composed of dense continuous spatial position points.

[0119] It should be further noted that this embodiment performs smoothing processing on the turning points in the conflict-free preliminary path point sequence, including:

[0120] S1051. Based on the conflict-free preliminary path point sequence, identify all turning points; specifically: sequentially traverse the conflict-free preliminary path point sequence, for each path point in the sequence except for the first and last points, calculate the forward vector formed by the path point and the previous point, and the backward vector formed by the path point and the next point; based on the dot product formula of the forward and backward vectors, calculate the angle between the two vectors; mark the path points with an angle less than a preset smoothing angle threshold as turning points to be smoothed.

[0121] S1052. Based on the index positions of all identified turning points, the conflict-free preliminary path point sequence is divided into multiple sub-path segments; specifically, the original sequence is divided into several continuous straight line segments using the starting point, all turning points, and the ending point of the entire sequence as dividing points; each straight line segment is defined by the path point coordinates of its starting and ending points; each turning point to be smoothed together with the ending point of the preceding straight line segment and the starting point of the following straight line segment constitutes a local turning point processing unit.

[0122] S1053. For each of the local transition processing units, a smooth transition curve is generated using a cubic Bézier curve fitting algorithm. Specifically, the endpoint of the preceding straight line segment in the local transition processing unit is taken as the starting point of the transition curve, and the starting point of the following straight line segment is taken as the endpoint of the transition curve. The position of the first control point is determined by the tangential direction at the endpoint of the preceding straight line segment, and the position of the second control point is determined by the tangential direction at the starting point of the following straight line segment. Based on the standard cubic Bézier curve parametric equation, a smooth curve connecting the preceding and following straight line segments is calculated using the starting point, the two control points, and the endpoint.

[0123] S1054. Based on the aforementioned minimum turning radius kinematic constraint, a curve adjustment algorithm based on curvature calculation and iterative optimization is used to process each generated transition curve segment, including:

[0124] Based on the standard parametric equation of a cubic Bézier curve and its first and second derivatives, the mathematical expression of the curvature of the transition curve with respect to the curve parameter variables is derived and obtained.

[0125] Based on the obtained curvature mathematical expression, a numerical optimization search algorithm is used to find the parameter point that maximizes the curvature value within the domain of the curve parameter variables. The numerical optimization search algorithm includes, but is not limited to, the golden section search method, which approaches the maximum value point by continuously narrowing the interval range.

[0126] Based on the maximum curvature value obtained from the search, the instantaneous radius of curvature corresponding to the maximum curvature value is calculated using the conversion formula between curvature and radius.

[0127] Verify whether the instantaneous radius of curvature calculated is greater than or equal to the minimum turning radius value specified in the kinematic constraint of the minimum turning radius.

[0128] If the verification result is unsatisfactory, a curve shape optimization process based on iterative adjustment of control point positions is executed: Based on the direction vector from the start point of the transition curve to the first control point, and the direction vector from the end point of the transition curve to the second control point, the adjustment directions of the two control points are determined; according to a preset, identical proportional coefficient, the two control points are moved along their respective direction vectors towards the midpoint of the line connecting the start and end points to reduce the curvature of the curve. This adjustment process changes the shape of the control polygon of the Bézier curve, thereby generating a new transition curve with lower curvature.

[0129] Repeating the above complete steps from deriving and obtaining the curvature mathematical expression to performing iterative adjustments of the control point position constitutes a closed-loop iterative optimization process. The termination condition of this process is: the latest generated transition curve satisfies the minimum turning radius kinematic constraint, or the preset maximum number of iterations has been reached, and finally, the optimized transition curve segment that satisfies the kinematic curvature constraint is output.

[0130] S1055. Based on all transition curve segments that satisfy the constraints and the original straight line segments that have not been replaced, a complete smooth path is generated using path splicing and uniform parameterized resampling methods. Specifically, the original straight line segments and the optimized transition curve segments are connected end to end in the order of the original path to form a continuous path composed of a mixture of straight lines and curves. The mixed path is then subjected to dense linear interpolation sampling with a fixed arc length step or parameter step, and the final output is a smooth path point sequence composed of a series of dense and continuous spatial location points.

[0131] S106. Continuously acquire task update signals or fault signals from the scheduling system or the vehicle's underlying layer; it should be further noted that the task update signals continuously acquired in this embodiment include at least the following structured data parsed from the new task instructions issued by the central scheduling system: the coordinates of the new task's target point, the task priority value, the task type identifier, and the expected completion timestamp; the fault signals continuously acquired include at least the following real-time status variables reported by the vehicle's underlying controller through the vehicle communication network: the vehicle's unique identifier, the fault type code, the fault occurrence timestamp, the fault severity level, and possibly associated abnormal sensor readings or actuator status flags.

[0132] S107. Identify the first set of automated guided vehicles whose paths need to be adjusted due to the task update signal or fault signal.

[0133] It should be further explained that the first set of automated guided vehicles (AGVs) whose paths need to be adjusted in this embodiment includes:

[0134] Based on the original data packet of the task update signal or fault signal, extract the signal type identifier and signal load data; the signal type identifier is a predefined enumeration value used to distinguish between task update events and vehicle fault events; the signal load data includes at least: for task update signals, the unique identifier of the target vehicle of the new task encapsulated in its load data; for fault signals, the unique identifier of the faulty vehicle encapsulated in its load data and the fault location represented by two-dimensional coordinates.

[0135] Based on the extracted signal type identifier, a conditional branching processing structure is used to perform logical judgments and collect vehicle identifiers, specifically as follows:

[0136] If the signal type identifier is equal to the enumeration value corresponding to the task update event, then the first collection sub-process is executed: First, based on the unique identifier of the target vehicle, the corresponding vehicle is added to an initially empty set of vehicle identifiers to be adjusted; Second, based on the preset path-task association rule base, all remaining assigned tasks that conflict with the new task in terms of path resources or time window are retrieved, and the execution vehicle identifiers corresponding to the remaining assigned tasks are obtained, and the execution vehicle identifiers are added to the set of vehicle identifiers to be adjusted.

[0137] If the signal type identifier is equal to the enumeration value corresponding to the vehicle malfunction event, then the second collection sub-process is executed: First, based on the unique identifier of the malfunctioning vehicle, the corresponding vehicle is added to the set of vehicle identifiers to be adjusted; second, based on the malfunction location coordinates and a predefined malfunction influence radius, a line segment and circular region intersection detection algorithm based on the ray method is used to traverse the current planned paths of all remaining automated guided vehicles in the system except for the malfunctioning vehicle; for each path, it is discretized into a series of continuous line segments, and it is detected whether any line segment intersects with a circular region centered on the malfunction point and with the malfunction influence radius as the radius; if an intersection is detected, the identifier of the vehicle to which the corresponding path belongs is added to the set of vehicle identifiers to be adjusted.

[0138] Based on the set of vehicle identifiers to be adjusted obtained after the conditional branch processing structure is completed, the set is processed by combining the union operation and deduplication algorithm in set operations; specifically, the sets that may be generated by the two sub-processes are merged, and duplicate vehicle identifiers are eliminated, and finally the first set of automated guided vehicles is generated and output.

[0139] S108. Based on the current task priority of each automated guided vehicle in the first automated guided vehicle set, generate an execution order queue for path resource reallocation.

[0140] It should be further explained that the execution order queue for generating path resource reallocation in this embodiment includes:

[0141] Based on the unique identifier of each automated guided vehicle in the first automated guided vehicle set, the current task priority value corresponding to each identifier is obtained from the task status database of the central scheduling system.

[0142] Based on the obtained current task priority value, all automated guided vehicles (AGVs) in the first AGV set are sorted. The sorting rule is as follows: AGVs with higher current task priority values ​​are given priority in the order. For AGVs with the same current task priority value, their position in the order is determined according to the lexicographical order of the vehicle identifier or the order of the task reception timestamp.

[0143] Based on the sorting results, the unique identifiers of the sorted automated guided vehicles are stored sequentially into a first-in-first-out (FIFO) sequential queue, and an execution order queue for path resource reallocation is generated and output.

[0144] S109. Based on the execution order queue, and using the latest task target point and real-time position of each automated guided vehicle (AGV) in the first AGV set, generate a first candidate smooth path point sequence for each AGV. Specifically, based on the execution order queue, use a linear traversal algorithm to sequentially obtain the unique identifier of each AGV to be processed; based on the unique identifier, use a key-value pair index retrieval algorithm to obtain the latest task target point coordinates and real-time position coordinates of the AGV from the first AGV set; based on the real-time position coordinates and the latest task target point coordinates, use a grid map-based A... The path planning algorithm calculates an initial path connecting two points in a predefined warehouse environment grid map. Based on the initial path, a vector angle determination algorithm is used to identify turning points in the path, and then a cubic Bézier curve fitting algorithm is used to smooth the identified turning points, generating multiple smooth curve segments. Based on the kinematic constraint of the minimum turning radius of the automated guided vehicle, a curvature calculation and iterative optimization algorithm is used to process each smooth curve segment. By solving the curvature of each point on the curve, the instantaneous radius of curvature is derived, and it is checked whether the instantaneous radius of curvature is not less than the minimum turning radius value. If it does not meet the requirement, the position of the Bézier curve control point is iteratively adjusted until the constraint is met. The smooth curve segments that meet the constraints are seamlessly spliced ​​with the straight segments in the initial path in the original path order to form a complete smooth path. Finally, an equal arc length discretization sampling algorithm is used to densely sample the smooth path, generating a first candidate smooth path point sequence composed of a series of dense and continuous spatial location points.

[0145] S110. Detect the spatial positional conflict between the first candidate smooth path point sequence of each automated guided vehicle and the first candidate smooth path point sequence corresponding to the remaining automated guided vehicles, and generate a set of conflict-free first smooth path point sequences by combining the preset conflict resolution strategy.

[0146] It should be further explained that this embodiment, in conjunction with a preset conflict resolution strategy, generates a set of conflict-free first smooth path point sequences, including:

[0147] Based on the first candidate smooth path point sequence of all automated guided vehicles and their preset dimensions, a geometric conflict detection algorithm based on time window and separation axis theorem is used to perform cyclic detection between all vehicles to identify vehicle pairs with spatial position conflicts and their corresponding conflict path segments.

[0148] Based on the identified conflicts, the conflicting vehicle pairs are traversed, and a time delay strategy is applied to one of the vehicles according to the preset conflict resolution priority rules: that is, a stationary waiting time of preset duration is inserted before the starting point or conflict segment of its original path point sequence, and its time-position sequence based on the uniform motion model is recalculated from the end of the waiting time.

[0149] Based on the updated time-position sequence after applying the time delay strategy, the geometric conflict detection algorithm based on time windows and the separation axis theorem is re-executed to verify whether the original conflict has been eliminated. If the conflict still exists, local path replanning is initiated for the vehicle: the starting point of the conflict path segment is taken as the new planning starting point, the original task target point is taken as the ending point, and the space occupied by the path of the other conflicting vehicle during the conflict time period is marked as a temporary obstacle, using A... The path planning algorithm calculates a new path segment that bypasses the area.

[0150] Based on the newly calculated local path segment, the same curve smoothing and constraint verification method as that used to generate the first candidate smooth path point sequence is adopted, namely, using Bézier curve fitting and ensuring that the minimum turning radius kinematic constraint is met, to generate a new smooth path segment and replace the conflict segment in the original path.

[0151] After completing the iterative processing of all identified conflicts, the same conflict detection algorithm is used to perform a final verification of the paths of all vehicles globally to ensure that no new conflicts are generated. Finally, the updated set of first smooth path point sequences for all vehicles that have been confirmed to be conflict-free is output.

[0152] S111. Based on the set of conflict-free first smooth path point sequences, the corresponding automated guided vehicle (AGV) tag information is embedded into the corresponding AGV.

[0153] This process achieves real-time and basic optimality in task allocation by acquiring a comprehensive dataset containing real-time status and task information, and calculating the initial shortest path for each vehicle in a predefined grid map. Furthermore, it performs collision detection and probability assessment through simulated driving, and corrects and replans the paths based on the collision probability, effectively generating a collision-free initial path point sequence. This avoids the risk of collisions during multi-vehicle operation, ensuring the system's operational safety and efficiency. Next, it smooths the paths using vehicle kinematic constraints, generating a smooth path point sequence with continuous curvature, ensuring the automated guided vehicles (AGVs) can follow their tracks smoothly and efficiently. Simultaneously, it dynamically adjusts the paths by continuously monitoring task and fault signals, and employs differentiated control command generation and constraint shaping strategies for track and off-track areas. This ensures the continuity of vehicle position, heading, and curvature, and the smooth transition of control commands when switching between mixed areas. Ultimately, this improves the overall reliability, path safety, and operational smoothness of the multi-AGV system in complex warehousing environments.

[0154] S2. For the current target point in the path point sequence, determine its region type: if it is located in a preset track region, generate a first control command for path tracking; if it is located in a preset non-track region, use the pose and curvature at the exit of the adjacent previous track region as the initial reference for trajectory planning, and start the autonomous trajectory planning process.

[0155] It should be further explained that this embodiment, based on a predefined two-dimensional array of warehouse environment raster map data, employs a traversal and attribute labeling algorithm to assign a region type attribute to each raster cell in the map; specifically: sequentially traversing the raster... Figure 2 For each element in the dimension array, read its predefined original attribute value; if the original attribute value indicates that the grid cell is covered by the physical guidance path, or its coordinates fall within the track geometry polygon defined by the system layout design file, then mark the grid cell's region type attribute as a track region; otherwise, mark the grid cell's region type attribute as a non-track region.

[0156] It should be further explained that the first control command generated in this embodiment for path tracking includes:

[0157] S201. Based on the current real-time position coordinates and heading angle of the automated guided vehicle, and the coordinates of the preset track reference path points in the path point sequence, obtain the lateral position deviation and heading angle deviation values ​​of the vehicle's current real-time position relative to the preset track path, specifically:

[0158] Based on the current real-time position coordinates of the automated guided vehicle and the coordinates of the previous path point closest to the current real-time position in the preset track reference path point sequence, the displacement vector from the previous path point to the current position of the vehicle is calculated.

[0159] Based on the coordinates of the previous path point closest to the current real-time position in the preset track reference path point sequence and the coordinates of its subsequent path points, the path direction vector from the previous point to the next point is calculated.

[0160] Based on the calculated displacement vector and path direction vector, a vector projection calculation method is used, specifically including vector dot product operation and vector magnitude calculation, to project the displacement vector onto the path direction vector to obtain the longitudinal projection component; then, using vector subtraction, the longitudinal projection component is subtracted from the displacement vector to obtain the lateral displacement vector.

[0161] Based on the magnitude of the lateral displacement vector, and considering the relationship between the direction of the path direction vector and the direction of the lateral displacement vector, a sign determination method is used to calculate the lateral position deviation value. Specifically, the unit normal vector of the path direction vector is calculated, and the dot product of the lateral displacement vector and the unit normal vector is calculated. The sign of the dot product result determines the direction of the lateral position deviation, and its absolute value is the magnitude of the lateral position deviation.

[0162] Based on the current heading angle data obtained by the inertial measurement unit of the automated guided vehicle (AGV) and the path direction vector, the heading angle deviation value is calculated using a vector angle calculation method. Specifically, the angle between the path direction vector and the reference axis of the global coordinate system is calculated to obtain the path reference heading angle. The difference between the vehicle's current heading angle and the path reference heading angle is calculated, and after angle normalization to a preset range, the final heading angle deviation value is obtained.

[0163] The lateral position deviation value and heading angle deviation value are output as inputs to the control algorithm.

[0164] S202. Based on the calculated lateral position deviation and heading angle deviation, a proportional-integral-derivative (PI-DE) control algorithm is used to calculate the initial steering control quantity for correcting the deviation; wherein, based on the calculated current lateral position deviation and current heading angle deviation, a preset proportional coefficient is multiplied to obtain the proportional term output value; the current lateral position deviation and current heading angle deviation are calculated by the aforementioned vector geometry method; based on the stored historical deviation sequence, an accumulation algorithm is used to sum the historical lateral position deviation and historical heading angle deviation, respectively, to obtain the historical cumulative lateral and heading deviations. The historical cumulative deviation values ​​are multiplied by preset integral coefficients to obtain the integral term output value. To prevent integral saturation, a preset amplitude limit is applied to the integral term output value. Based on the current lateral position deviation value and current heading angle deviation value obtained in the current sampling period, and the previous lateral position deviation value and previous heading angle deviation value stored in the previous sampling period, the forward difference algorithm is used to calculate the difference: the current lateral position deviation value is subtracted from the previous lateral position deviation value to obtain the lateral position deviation difference; the current heading angle deviation value is subtracted from the previous heading angle deviation value to obtain the heading angle deviation difference. Each deviation difference is divided by the sampling period duration to obtain the corresponding lateral position deviation change rate and heading angle deviation change rate. The lateral position deviation change rate and heading angle deviation change rate are multiplied by preset differential coefficients to obtain the differential term output value. The proportional, integral, and derivative output values ​​calculated for lateral and heading directions respectively are linearly superimposed to obtain the initial lateral control quantity and the initial heading control quantity. The initial lateral control quantity and the initial heading control quantity are then output as the initial steering control quantity used to correct deviations.

[0165] S203. Based on the reference speed value defined in the path point sequence, and combined with the absolute values ​​of the lateral position deviation value and the heading angle deviation value, the reference speed value is dynamically corrected to obtain the final speed control command.

[0166] It should be further explained that this embodiment dynamically corrects the reference velocity values ​​defined in the path point sequence, including:

[0167] Based on the calculation results of the lateral position deviation and heading angle deviation, the absolute values ​​of the lateral position deviation and heading angle deviation are obtained respectively.

[0168] Based on preset lateral deviation thresholds and heading deviation thresholds, the absolute values ​​of the lateral position deviation and heading angle deviation are verified. Specifically, if the absolute value of the lateral position deviation is greater than or equal to the lateral deviation threshold, or the absolute value of the heading angle deviation is greater than or equal to the heading deviation threshold, then it is determined that speed compensation is required.

[0169] When it is determined that speed compensation is required, the larger of the absolute values ​​of the lateral position deviation and the heading angle deviation is selected as the dominant deviation value.

[0170] Based on the dominant deviation value and the preset deviation-speed compensation coefficient mapping table, the speed compensation coefficient is calculated using a linear interpolation algorithm. The deviation-speed compensation coefficient mapping table defines a monotonically decreasing function relationship from the dominant deviation value to the speed compensation coefficient, wherein the speed compensation coefficient ranges from 0 to 1.

[0171] Multiply the reference speed value by the calculated speed compensation coefficient to obtain the preliminary speed command after dynamic correction.

[0172] Based on the physical constraint of the maximum rated acceleration of each automated guided vehicle, the initial speed command is smoothed by combining a first-order low-pass filtering algorithm to limit the rate of change of the speed command and obtain the speed control command.

[0173] S204. Based on the physical constraints of the maximum steering angular velocity and maximum acceleration of the automated guided vehicle actuator, a limiting filtering algorithm is used to constrain the initial steering control quantity and the speed control command to obtain constrained steering control command and speed control command.

[0174] It should be further explained that this embodiment uses a limiting filtering algorithm to constrain the initial steering control quantity and the speed control command, including:

[0175] Based on the initial steering control quantity and the final steering control command of the previous control cycle stored in memory, the change in the current steering control command is calculated; based on the fixed sampling period set by the control system, the change in the current steering control command is divided by the sampling period duration to calculate the instantaneous rate of change of the steering control command.

[0176] Based on the physical constraint of the maximum steering angular velocity defined in the vehicle actuator technical specifications, a numerical comparison logic is used to determine whether the instantaneous rate of change of the calculated steering control command exceeds the preset maximum steering angular velocity value. If the absolute value of the instantaneous rate of change is greater than the maximum steering angular velocity value, a limiting function is used to restrict the instantaneous rate of change to within the range of the positive and negative maximum steering angular velocity values. Based on this limited transient rate of change, the allowable steering control quantity in the current control cycle is recalculated and used as the intermediate steering control quantity. Otherwise, the initial steering control quantity is directly used as the intermediate steering control quantity.

[0177] Based on the speed control command and the final speed control command of the previous control cycle stored in memory, the change in the current speed control command is calculated by subtraction. Based on the fixed sampling period, the change is divided by the sampling period duration to calculate the instantaneous rate of change of the speed control command, i.e., acceleration.

[0178] Based on the physical constraints of maximum acceleration and maximum deceleration defined in the vehicle drive system technical specifications, a numerical comparison logic is used to determine whether the calculated acceleration exceeds the allowable range. If the acceleration is greater than the maximum acceleration value, a limiting function is used to limit the acceleration to the maximum acceleration value. If the acceleration is less than the maximum deceleration value, i.e., a negative maximum acceleration value, a limiting function is used to limit the acceleration to the maximum deceleration value. Based on this limited acceleration value, the allowable speed control command within the current control cycle is recalculated and used as an intermediate speed control command. Otherwise, the speed control command is directly used as an intermediate speed control command.

[0179] The intermediate steering control quantity and the intermediate speed control command are smoothed using a first-order low-pass filtering algorithm to filter out high-frequency noise and further ensure the continuity of command changes. The control quantity obtained after filtering is updated and stored as the new final steering control command and final speed control command, and then output.

[0180] S205. The constrained steering control command and speed control command are encapsulated and combined to generate the first control command for path tracking.

[0181] In this embodiment, precise lateral position and heading angle deviations are obtained through vector geometry calculations within the track area. A preliminary steering control quantity is generated based on a proportional-integral-derivative (PI-DE) control algorithm, achieving high-precision tracking of the preset track path. Furthermore, the reference speed is dynamically compensated and smoothed by combining lateral and heading deviations. Before the final output, steering and speed commands are subject to strict amplitude-limiting filtering constraints based on the physical limits of the actuators, ensuring the smoothness, feasibility, and vehicle stability of the control commands. This series of steps not only guarantees the tracking accuracy and operational safety of the automated guided vehicle within the track area, but more importantly, by generating smooth and physically constrained final control commands, it provides a reliable guarantee that the vehicle will reach the expected pose and curvature state at the track area exit. This lays a crucial foundation for seamless connection with the starting point of the smooth trajectory in the non-track area, ultimately improving the vehicle's continuity and overall control performance throughout the mixed-area operation.

[0182] S3. Using the initial reference value as the starting boundary condition, obtain the smooth trajectory of the non-track segment leading to the current target point, and generate a second control command for trajectory tracking; the current target point refers to the continuous spatial location point that the vehicle needs to go to in the path point sequence dynamically allocated to the automated guided vehicle; during the control process, this point dynamically switches as the vehicle position is updated: when the vehicle arrives at the current target point, that is, the end point of a region segment, the controller updates the next point in the path point sequence as the new current target point; within the track area, this point is directly used for path tracking; in the non-track area, when the end position of the non-track area is reached, the current target point is the end position of the trajectory planning, used to generate a smooth trajectory connecting the track area exit to this point; therefore, the current target point not only represents the real-time task target of the vehicle in different areas, but also serves as the basis for switching between region segments and trajectory generation.

[0183] It should be further explained that the generation of the second control command for trajectory tracking in this embodiment includes:

[0184] S301. Using the pose and curvature recorded at the exit of the adjacent preceding track region as initial reference values, and the coordinates of the current target point in the non-track region, trajectory planning is performed to generate a parametric smooth trajectory connecting the start and end points, specifically:

[0185] S3011. Based on the coordinates of the starting point and the coordinates of the current target point, obtain the straight-line distance between the two points as the estimated total arc length of the trajectory.

[0186] S3012. Based on the starting heading angle, the unit tangent vector at the starting point is calculated using the trigonometric function transformation method.

[0187] S3013. Using the starting point coordinates as the initial point, the unit tangent vector as the initial direction, and the current target point coordinates as the ending point, and combining the preset trajectory smoothness constraints, construct a set of cubic spline curve equations to describe the relationship between the position coordinates and the arc length parameter. The set of equations ensures that the position and tangent direction of the curve are continuous at the starting and ending points, and that the second derivative is continuous at the internal nodes.

[0188] S3014. Based on the constructed cubic spline curve equation system combined with the matrix solving algorithm, calculate the polynomial coefficient matrix of the cubic spline curve; the matrix solving algorithm transforms the equation system into a tridiagonal linear equation system, and uses the chasing method for numerical solution.

[0189] S3015. Based on the polynomial coefficient matrix, a parameterized trajectory generation method is adopted, with arc length as the independent variable, and within the range from zero to the estimated total arc length, the coordinates of the corresponding trajectory points and the tangent direction are calculated with a preset dense parameter step size, and the parameterized smooth trajectory connecting the starting point and the ending point is generated and output.

[0190] S302. Based on the parameterized smooth trajectory, calculate the feedforward control quantity required for trajectory tracking using the trajectory parameter differentiation method, and obtain the feedforward speed command; specifically:

[0191] S3021. Based on the polynomial coefficient matrix of the parameterized smooth trajectory, calculate the first and second derivatives of the X-coordinate component of the trajectory with respect to the arc length parameter, and simultaneously calculate the first and second derivatives of the Y-coordinate component of the trajectory with respect to the arc length parameter, to obtain the first and second derivative sequences of each component.

[0192] S3022. Based on the first derivative sequence of the X-coordinate components and the Y-coordinate components, calculate the unit tangent vector sequence corresponding to each point on the trajectory. Specifically, for each parameter point, construct a two-dimensional vector using its first derivative value in the X-direction and the first derivative value in the Y-direction as components, calculate the magnitude of the two-dimensional vector, and finally divide each component by the magnitude to obtain the unit tangent vector.

[0193] S3023. Based on the first and second derivative sequences of the X and Y coordinate components, the curvature value sequence of each point on the trajectory is calculated using the two-dimensional plane curve curvature formula. The calculation formula is: curvature equals the product of the first derivative in the X direction and the second derivative in the Y direction minus the product of the first derivative in the Y direction and the second derivative in the X direction, and then divided by the cube of the sum of the squares of the first derivative in the X direction and the first derivative in the Y direction.

[0194] S3024. Based on the curvature value sequence and the kinematic model of the automated guided vehicle, a feedforward steering angle command sequence is calculated; specifically: according to the vehicle kinematic model, under the low-speed assumption, the expected front wheel steering angle of the vehicle is approximately equal to the trajectory curvature multiplied by the vehicle's wheelbase; each curvature value in the curvature value sequence is multiplied by the vehicle's preset wheelbase parameter to obtain the corresponding feedforward steering angle command.

[0195] S3025. Based on the reference velocity value preset for the current target point on the parameterized smooth trajectory, directly obtain the feedforward velocity command.

[0196] S303. Based on the current real-time position and heading angle of the automated guided vehicle, and the coordinates and tangent direction of the projection point closest to the current real-time position on the parameterized smooth trajectory, the lateral position error and heading error of the trajectory tracking are obtained, specifically:

[0197] S3031. Based on the current real-time position coordinates of the automated guided vehicle, the nearest neighbor search algorithm is used to calculate and find the trajectory point with the smallest Euclidean distance to the current real-time position coordinates in the discrete point sequence of the parameterized smooth trajectory. The trajectory point with the smallest distance is defined as the projection point, and the coordinates of the projection point and the unit tangent vector stored in the projection point are obtained.

[0198] S3032. Based on the current real-time position coordinates and the projection point coordinates, the error vector pointing from the projection point to the vehicle's current real-time position is calculated using vector subtraction.

[0199] S3033. Based on the unit tangent vector at the projection point, a unit normal vector perpendicular to the unit tangent vector is calculated using a vector rotation and normalization method; specifically, the two components of the unit tangent vector are swapped, one of the components is inverted, and then normalized to obtain the unit normal vector.

[0200] S3034. Based on the error vector and the unit normal vector, calculate the projection length of the error vector in the direction of the unit normal vector, and define the projection length as the lateral position error; the sign of the projection length is determined by the sign of the dot product of the error vector and the unit normal vector. Specifically, the dot product of the error vector and the unit normal vector yields a scalar value that represents the magnitude and sign of the projection length: if the dot product is positive, it indicates that the vehicle's current position is on the side of the positive direction of the unit normal vector of the reference trajectory; if the dot product is negative, it indicates that the vehicle is on the side of the opposite direction of the unit normal vector. This sign is used to distinguish the direction of the lateral position error, thereby achieving a positive or negative indication of the deviation.

[0201] S3035. Based on the current heading angle obtained by the inertial measurement unit of the automated guided vehicle, and the angle between the unit tangent vector at the projection point and the preset global coordinate system reference axis, calculate the angle difference between the two; specifically: take the angle of the unit tangent vector as the desired heading angle, calculate the difference between the current heading angle and the desired heading angle, and process the difference to a preset range of -180° to 180° using an angle normalization algorithm to obtain the heading error;

[0202] S3036. Output the lateral position error and heading error as inputs for trajectory tracking feedback control.

[0203] S304. Based on the lateral position error and heading error, the optimal state feedback gain matrix for error elimination is obtained by solving the algebraic Riccati equation. The feedback steering angle adjustment is obtained by multiplying the lateral position error and heading error by the corresponding gain value. It should be further noted that in this embodiment, the optimal state feedback gain matrix for error elimination is obtained by solving the algebraic Riccati equation, including:

[0204] S3041. Based on a simplified kinematic model of the automated guided vehicle (AGV), a system state equation is constructed with lateral position error and heading error as state variables and front wheel angle as control input. The system state matrix A and system control input matrix B are then extracted from this equation. The simplified kinematic model of the AGV in this embodiment typically uses a bicycle model based on geometric relationships. Its core principle is to ignore complex dynamics such as tire sideslip, simplifying the four-wheeled vehicle into a two-wheeled model. Using front wheel angle and rear wheel drive as inputs, mathematical relationships are established between vehicle position, heading angle, speed, and steering. This simplified kinematic model is primarily used for the design and analysis of control systems. Specifically, in trajectory tracking control, it provides state-space equations describing the dynamics of the vehicle's lateral and heading errors for optimal controllers such as linear quadratic regulators (LQRs) to solve for the optimal state feedback gain matrix. Simultaneously, it provides a direct mapping relationship from path curvature to the desired steering angle for the feedforward controller, thereby collaboratively achieving high-precision trajectory tracking.

[0205] S3042. Based on the performance requirements of trajectory tracking, a state error weight matrix Q and a control input weight matrix R are preset. Specifically, the state error weight matrix Q is constructed as follows: according to the different requirements of the trajectory tracking task for lateral position control accuracy and heading alignment accuracy, a lateral position error penalty coefficient and a heading error penalty coefficient are preset, and a 2×2 diagonal matrix is ​​constructed using the lateral position error penalty coefficient and the heading error penalty coefficient as diagonal elements. Specifically, the control input weight matrix R is constructed as follows: according to the constraints on the smoothness and energy consumption of the steering actuator, a front wheel angle change penalty coefficient is preset, and this scalar value or a diagonal matrix with it as elements is directly defined as the control input weight matrix R. The specific values ​​of the diagonal coefficients in the state error weight matrix Q and the coefficients in the control input weight matrix R in this embodiment are preset and calibrated based on the overall system performance requirements, by balancing the trajectory tracking accuracy, the system dynamic response speed, and the control energy consumption of the front wheel steering mechanism. In the parameter tuning of the trajectory tracking controller, this embodiment weighs the balance between the tracking accuracy of the desired trajectory and the control cost incurred in achieving the tracking. The specific method of weighing the factors is as follows: based on the geometric characteristics of the target trajectory, such as curvature changes, the ratio of straight lines to curves, and task requirements, the penalty coefficient is systematically adjusted through control simulation and experimental verification. Increasing the coefficient of the state error weight matrix Q will improve the system's ability to correct trajectory deviations and the convergence speed, but may lead to drastic changes in the control quantity. Increasing the coefficient of the control input weight matrix R will suppress the amplitude and rate of change of the steering angle, making the control action smoother and the energy consumption lower, but will reduce the tracking response speed and accuracy of the system. Finally, through iterative optimization, a set of coefficients that achieves the optimal balance between tracking error and control energy consumption is determined.

[0206] S3043. Based on the system state matrix A, system control input matrix B, state error weight matrix Q, and control input weight matrix R, construct the continuous-time algebraic Riccati equation, specifically as follows:

[0207] The continuous-time algebraic Riccati equation is solved by numerical iteration to obtain its symmetric positive definite solution matrix P. The numerical iteration method in this embodiment is the Schur decomposition method, which includes the following steps: constructing a Hamiltonian matrix based on the system matrix, performing Schur decomposition on the Hamiltonian matrix to obtain a stable invariant subspace, and then calculating the symmetric positive definite solution matrix P from the subspace.

[0208] Based on the obtained symmetric positive definite solution matrix P, system control input matrix B, and control input weight matrix R, the gain calculation formula K=R is used. -1 B T P is used to calculate the optimal state feedback gain matrix K.

[0209] The calculated optimal state feedback gain matrix K is stored in memory as the core parameter of the trajectory tracking feedback control law, which is used to map the real-time calculated lateral position error and heading error into the feedback steering angle adjustment.

[0210] S305. The feedforward steering angle command is superimposed with the feedback steering angle adjustment amount to obtain a preliminary synthetic steering control command.

[0211] S306. Based on the maximum steering angle rate constraint of the automated guided vehicle actuator, the same amplitude limiting filtering algorithm as that used in the generation of the first control command is used to constrain the synthesized steering control command to obtain the constrained steering control command.

[0212] It should be further noted that this embodiment uses the same amplitude limiting filtering algorithm as in the generation of the first control command to constrain the synthesized steering control command, including:

[0213] S3061. Obtain the synthetic steering control command at the current moment, and obtain the final steering control command after constraint processing at the previous moment;

[0214] S3062. Based on the synthesized steering control command at the current moment and the final steering control command at the previous moment, calculate the first command increment between the two.

[0215] S3063. Determine whether the absolute value of the first instruction increment is greater than a preset first increment limiting threshold; if the absolute value of the first instruction increment is greater than the first increment limiting threshold, then based on the first increment limiting threshold and the sign of the first instruction increment, limit the first instruction increment to obtain the first limited increment; otherwise, directly use the first instruction increment as the first limited increment.

[0216] S3064. The first intermediate command value is obtained by summing the final steering control command of the previous moment and the first amplitude limit increment.

[0217] S3065. Determine whether the first intermediate command value exceeds the preset upper or lower limit of the final control command; if the first intermediate command value is greater than the upper limit of the final control command, output the upper limit of the final control command as the final steering control command at the current moment; if the first intermediate command value is less than the lower limit of the final control command, output the lower limit of the final control command as the final steering control command at the current moment; if the first intermediate command value is within the interval defined by the upper and lower limits of the final control command, output the first intermediate command value directly as the final steering control command at the current moment, i.e., the steering control command after constraint processing.

[0218] S307. Based on the feedforward speed command, the same amplitude limiting filtering algorithm is used to smooth and limit the rate of change, generating a constrained speed control command.

[0219] It should be further explained that this embodiment uses an amplitude-limiting filtering algorithm to smooth and limit the rate of change of the feedforward speed command, including:

[0220] S3071. Obtain the feedforward speed command at the current moment and obtain the final speed control command output at the previous moment;

[0221] S3072. Based on the current feedforward speed command and the previous final speed control command, calculate the second command increment between the two.

[0222] S3073. Determine whether the absolute value of the second instruction increment is greater than a preset second increment limiting threshold; if the absolute value of the second instruction increment is greater than the second increment limiting threshold, then based on the second increment limiting threshold and the sign of the second instruction increment, limit the second instruction increment to obtain the second limited increment; otherwise, directly use the second instruction increment as the second limited increment.

[0223] S3074. The second intermediate command value is obtained by summing the final speed control command of the previous moment with the second limit increment.

[0224] S3075. Determine whether the second intermediate command value exceeds the preset upper or lower limit of the final speed command; if the second intermediate command value is greater than the upper limit of the final speed command, output the upper limit of the final speed command as the final speed control command at the current moment; if the second intermediate command value is less than the lower limit of the final speed command, output the lower limit of the final speed command as the final speed control command at the current moment; if the second intermediate command value is within the interval defined by the upper limit of the final speed command and the lower limit of the final speed command, output the second intermediate command value directly as the final speed control command at the current moment, i.e., the constrained speed control command.

[0225] S308. The constrained steering control command and the constrained speed control command are encapsulated and combined to generate the second control command for non-track area trajectory tracking.

[0226] In this embodiment, within the non-track area, the pose and curvature precisely recorded at the track area exit are used as initial reference values. Combined with the target point coordinates, a cubic spline curve is used for trajectory planning. This ensures that the generated smooth trajectory in the non-track segment achieves strict continuity in position, heading, and curvature between the starting point and the end point of the track segment, laying the foundation for smooth switching. Based on this parameterized smooth trajectory, the feedforward control quantity is precisely calculated through differentiation. Combined with optimal feedback control based on a linear quadratic regulator, tracking errors are eliminated in real time, forming a precise trajectory tracking control law. Finally, by applying amplitude-limiting filtering constraints that match the capabilities of the physical actuators to the synthesized steering control commands, the smoothness and feasibility of the control commands are ensured. This series of steps works synergistically, enabling the automated guided vehicle (AGV) to autonomously, stably, and efficiently travel to the target point along a smooth trajectory that satisfies kinematic constraints, geometric continuity, and precise control after leaving the fixed track. This significantly improves the vehicle's flexibility, smoothness, and overall control performance in unstructured areas.

[0227] S4. The first control command is constrained and shaped to obtain a constrained and shaped first control command, and a second control command is generated based on the initial reference value and the current target point, so that at the junction of the track area and the non-track area, the path tracking endpoint state achieved by the constrained and shaped first control command and the starting state of the non-track segment smooth trajectory planned by the second control command satisfy the preset continuity rules; it should be further noted that the continuity rules in this embodiment include at least position, heading angle and curvature continuity, and the constraint shaping includes limiting the turning rate and acceleration;

[0228] It should be further explained that, in this embodiment, at the junction of the track area and the non-track area, the path tracking endpoint state implemented by the first control command after constraint shaping, and the starting state of the smooth trajectory of the non-track segment planned by the second control command, satisfy a preset continuity rule, including:

[0229] S401. Based on the path point sequence and the predefined area type attribute, obtain the connection point, specifically by: sequentially traversing each path point in the path point sequence, and querying the area type attribute corresponding to each path point based on the predefined warehouse environment grid map; for the path point whose first area type attribute is marked as a non-track area, determine and record the preceding path point in the path point sequence as the connection point.

[0230] S402. Based on the path point sequence, obtain the initial reference value at the connection point; specifically:

[0231] S4021. Based on the coordinates of the connecting point and the coordinates of subsequent path points in the path point sequence, calculate the direction vector from the connecting point to the subsequent path points, and calculate the heading angle at the connecting point using the arctangent function based on the direction vector, specifically:

[0232] Based on the index position of the path point sequence and the connecting point, subsequent path points are obtained; specifically, according to the index number of the connecting point in the path point sequence, a sequential index query method is used to obtain the path point corresponding to the index number plus one, the path point is defined as the subsequent path point, and the coordinates of the subsequent path point are obtained.

[0233] Based on the coordinates of the connecting point and the coordinates of the subsequent path points, a direction vector is calculated; specifically: the X coordinate of the subsequent path point is subtracted from the X coordinate of the connecting point to obtain the X-axis direction difference; the Y coordinate of the subsequent path point is subtracted from the Y coordinate of the connecting point to obtain the Y-axis direction difference; the X-axis direction difference and the Y-axis direction difference together constitute a two-dimensional vector, which is defined as the direction vector from the connecting point to the subsequent path point.

[0234] Based on the direction vector, the heading angle at the connection point is calculated. Specifically, the difference in the Y-axis direction is used as the numerator, and the difference in the X-axis direction is used as the denominator. A division operation is performed to obtain the quotient. The quotient is used as an input parameter, and the four-quadrant arctangent function is called to calculate the original angle value in radians. The original angle value is multiplied by the radian-to-degree conversion coefficient to obtain the heading angle in degrees. Finally, an angle range normalization algorithm is used to adjust the heading angle to a preset standard range of 0 to 360° or -180° to 180°, and the result is output as the final heading angle at the connection point.

[0235] S4022. Based on the coordinates of the preceding path points, the coordinates of the connecting point, and the coordinates of the subsequent path points in the path point sequence, the curvature at the connecting point is calculated using a discrete curvature estimation algorithm.

[0236] S4023. The coordinates of the connection point, the calculated heading angle at the connection point, and the calculated curvature at the connection point are recorded together as the initial reference values.

[0237] S403. Based on the maximum steering angle rate constraint of the automated guided vehicle actuator, perform constraint shaping on the steering control command in the first control command used for path tracking; specifically including:

[0238] Obtain the steering control command for the current control cycle and the final steering control command output from the previous control cycle.

[0239] The steering command increment is calculated based on the steering control command of the current control cycle and the final steering control command of the previous control cycle.

[0240] Determine whether the absolute value of the steering command increment is greater than a preset first increment limit threshold, which is determined based on the maximum steering angle rate and the sampling period of the control system.

[0241] If the absolute value of the steering command increment is greater than the first increment limiting threshold, the steering command increment is limited based on the first increment limiting threshold and the sign of the steering command increment to obtain the limited steering command increment.

[0242] The steering control command after constraint shaping is obtained by summing the final steering control command of the previous control cycle and the incremental steering command after the amplitude limit.

[0243] S404. Based on the maximum acceleration constraint of the automated guided vehicle's actuators, perform constraint shaping on the speed control command in the first control command used for path tracking; specifically including:

[0244] Obtain the speed control command for the current control cycle, and obtain the final speed control command output from the previous control cycle.

[0245] The speed command increment is calculated based on the speed control command of the current control cycle and the final speed control command of the previous control cycle.

[0246] Determine whether the absolute value of the speed command increment is greater than a preset second increment limit threshold, which is determined based on the maximum acceleration and the sampling period of the control system.

[0247] If the absolute value of the speed command increment is greater than the second increment limiting threshold, then the speed command increment is limited based on the second increment limiting threshold and the sign of the speed command increment to obtain the limited speed command increment.

[0248] The speed control command after constraint shaping is obtained by summing the final speed control command of the previous control cycle with the speed command increment after the amplitude limit.

[0249] S405. Combine the constrained steering control command with the constrained speed control command to obtain the constrained first control command.

[0250] S406. Based on the initial reference value and the current target position, generate a second control command for trajectory tracking in the non-track area;

[0251] S407. Obtain the actual state of the automated guided vehicle at the connection point based on sensor feedback information, wherein the actual state includes the actual position, actual heading angle and actual curvature.

[0252] S408. Determine whether the actual state and the initial reference quantity satisfy a preset continuity rule; the continuity rule requires that the distance error between the actual position and the position in the initial reference quantity is less than a first error threshold, the difference between the actual heading angle and the heading angle in the initial reference quantity is less than a second error threshold, and the difference between the actual curvature and the curvature in the initial reference quantity is less than a third error threshold.

[0253] If the continuity rule is satisfied, the path tracking endpoint state implemented by the first control command after constraint shaping is determined to be continuous with the starting state of the non-track segment smooth trajectory planned by the second control command.

[0254] S5. Output the first control command after constraint shaping to the path tracking control loop for the track area, and output the second control command to the autonomous trajectory tracking control loop for the non-track area.

[0255] In this embodiment, based on the first and second control commands after constraint shaping, they are output to corresponding control loops. The first control command after constraint shaping is output to the path tracking control loop in the track area to drive the actuator of the automated guided vehicle (AGV) to strictly follow the preset track reference path and ensure that its actual state is consistent with the initial reference value when it arrives at the connection point. The second control command is output to the autonomous trajectory tracking control loop in the non-track area to control the AGV to track the parameterized smooth trajectory generated based on the initial reference value, thereby achieving autonomous movement from the connection point to the current target point. Through this step, the two control loops act on the vehicle based on their respective control commands, ensuring that the handover of control is smooth and continuous when the AGV crosses the track area and the non-track area, and that there are no sudden changes in the motion state.

[0256] This embodiment accurately identifies and obtains the connection points between track and non-track areas and their corresponding initial reference values, including position, heading angle, and curvature, by sequentially traversing the path point sequence, providing a precise connection benchmark for subsequent control. Then, based on the physical limits of the vehicle's actuators, the first control command for the track area is strictly constrained and shaped, effectively limiting the steering rate and acceleration, ensuring the predictability and smoothness of the path tracking endpoint state. Using this initial reference value as boundary conditions, the second control command for the non-track area is generated, ensuring that the planned trajectory meets strict continuity rules in terms of position, heading, and curvature at the starting and ending points. Finally, by outputting the shaped commands to the control loops of the corresponding areas, a smooth transfer of control and abrupt transitions in vehicle motion state are achieved, thereby significantly improving the overall consistency, stability, safety, and efficiency of the automated guided vehicle (AGV) operating in complex mixed track and non-track environments.

[0257] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments under the guidance of the present invention without departing from the spirit and scope of the claims. All of these variations are within the protection scope of the present invention.

Claims

1. A trajectory planning method for mixed track and non-track areas, characterized in that, include: Based on the real-time status and task information of the automated guided vehicles (AGVs), a path point sequence consisting of continuous spatial location points is dynamically assigned to each AGV. For the current target point in the path point sequence, determine its region type: if it is located in a preset track region, generate a first control command for path tracking; If located in a preset non-track area, the pose and curvature at the exit of the adjacent preceding track area are used as the initial reference for trajectory planning, and the autonomous trajectory planning process is initiated. Using the initial reference value as the starting boundary condition, a smooth non-track segment trajectory is obtained to reach the current target point, and a second control command for trajectory tracking is generated; The first control command is constrained and shaped to obtain a constrained and shaped first control command, so that at the junction of the track area and the non-track area, the path tracking endpoint state achieved by the constrained and shaped first control command and the starting state of the non-track segment smooth trajectory planned by the second control command satisfy the preset continuity rule. The first control command after constraint shaping is output to the path tracking control loop for the track area, and the second control command is output to the autonomous trajectory tracking control loop for the non-track area.

2. The trajectory planning method for mixed track and non-track areas as described in claim 1, characterized in that, The continuity rule includes at least position, heading angle and curvature continuity, and the constraint shaping includes limiting the turning rate and acceleration. The dynamic allocation of a path point sequence consisting of continuous spatial location points for each automated guided vehicle includes: Obtain a comprehensive dataset containing the real-time location coordinates of each automated guided vehicle, its current cargo status, the coordinates of the task target point, and the task priority; Based on the task target point coordinates and real-time location coordinates in the comprehensive dataset, an initial shortest path is calculated for each automated guided vehicle (AGV) from its real-time location to its task target point in a predefined warehouse environment grid map; the initial shortest path consists of a series of sequentially connected map grid center points.

3. The trajectory planning method for mixed track and non-track areas as described in claim 2, characterized in that, The method of dynamically allocating a path point sequence consisting of continuous spatial location points for each automated guided vehicle also includes: Based on the initial shortest path calculated for all automated guided vehicles (AGVs), and the preset dimensions and speed model of each AGV, the process of each AGV traveling along its initial shortest path is simulated to obtain the probability of spatial overlap or path intersection between any two vehicles within a preset time window. Based on the initial shortest path and the conflict probability, the initial shortest path with conflict is corrected to obtain a preliminary path point sequence without conflict for each automated guided vehicle. Based on the conflict-free initial path point sequence and the kinematic constraint of the minimum turning radius of the automated guided vehicle, the turning points in the conflict-free initial path point sequence are smoothed to generate a smooth path with continuous curvature, and the smooth path is discretized into a smooth path point sequence composed of continuous spatial position points.

4. The trajectory planning method for mixed track and non-track areas as described in claim 3, characterized in that, The method of dynamically allocating a path point sequence consisting of continuous spatial location points for each automated guided vehicle also includes: Continuously acquire task update signals or fault signals from the scheduling system or the vehicle's underlying system; Identify the first set of automated guided vehicles whose paths need to be adjusted due to the task update signal or fault signal; Based on the current task priority of each automated guided vehicle in the first automated guided vehicle set, an execution order queue for path resource reallocation is generated. Based on the execution order queue, and using the latest task target point and real-time position of each automated guided vehicle in the first automated guided vehicle set, a first alternative smooth path point sequence is generated for each automated guided vehicle. Detect spatial positional conflicts between the first candidate smooth path point sequence of each automated guided vehicle and the first candidate smooth path point sequence corresponding to the remaining automated guided vehicles, and generate a set of conflict-free first smooth path point sequences by combining a preset conflict resolution strategy. Based on the set of conflict-free first smooth path point sequences, the tag information of the corresponding automated guided vehicle is embedded into the corresponding automated guided vehicle.

5. The trajectory planning method for a mixed track and non-track area as described in claim 4, characterized in that, The generation of the first control command for path tracking includes: Based on the current real-time position coordinates and heading angle of the automated guided vehicle, and the coordinates of the preset track reference path points in the path point sequence, the lateral position deviation and heading angle deviation values ​​of the current real-time position of the automated guided vehicle relative to the preset track path are obtained. Based on the calculated lateral position deviation and heading angle deviation values, the initial steering control quantity used to correct the deviation is calculated.

6. The trajectory planning method for a mixed track and non-track area as described in claim 5, characterized in that, The generation of the first control command for path tracking also includes: Based on the reference speed value defined in the path point sequence, and combined with the absolute values ​​of the lateral position deviation value and the heading angle deviation value, the reference speed value is dynamically corrected to obtain the final speed control command. Based on the physical constraints of the maximum steering angular velocity and maximum acceleration of the automated guided vehicle actuator, the initial steering control quantity and the speed control command are constrained to obtain constrained steering control commands and speed control commands. The constrained steering control command and speed control command are encapsulated and combined to generate the first control command for path tracking.

7. The trajectory planning method for a mixed track and non-track area as described in claim 6, characterized in that, The generation of the second control command for trajectory tracking includes: Using the pose and curvature recorded at the exit of the adjacent preceding track region as initial references, and the coordinates of the current target point in the non-track region, a parametric smooth trajectory connecting the start and end points is generated. Based on the parameterized smooth trajectory, the feedforward control quantity for trajectory tracking is calculated using the trajectory parameter differentiation method to obtain the feedforward speed command; Based on the current real-time position and heading angle of the automated guided vehicle, as well as the coordinates and tangent direction of the projection point closest to the current real-time position on the parameterized smooth trajectory, the lateral position error and heading error of the trajectory tracking are obtained.

8. The trajectory planning method for a mixed track and non-track area as described in claim 7, characterized in that, The generation of the second control command for trajectory tracking also includes: Based on the lateral position error and heading error, the optimal state feedback gain matrix for eliminating the error is calculated. The lateral position error and heading error are multiplied by the gain value corresponding to the optimal state feedback gain matrix to obtain the feedback steering angle adjustment. The feedforward steering angle command is superimposed with the feedback steering angle adjustment to obtain the synthetic steering control command; Based on the maximum steering angle rate constraint of the automated guided vehicle actuator, the synthesized steering control command is constrained to obtain the constrained steering control command. The feedforward speed command is smoothed and its rate of change is limited to generate a constrained speed control command. The constrained steering control command and the constrained speed control command are encapsulated to generate a second control command for trajectory tracking in non-track areas.

9. A trajectory planning method for a mixed track and non-track area as described in claim 8, characterized in that, The path tracking endpoint state achieved by the first control command after constraint shaping, and the starting state of the non-track segment smooth trajectory planned by the second control command, satisfy a preset continuity rule, including: Based on the path point sequence and predefined region type attributes, the connection point is obtained, specifically by: sequentially traversing each path point in the path point sequence and querying the region type attribute corresponding to each path point based on the predefined warehouse environment grid map; marking the first path point whose region type attribute is a non-track region, and determining and recording the preceding path point in the path point sequence as the connection point. Based on the path point sequence, obtain the initial reference value at the connection point; Based on the maximum steering angle rate constraint of the automated guided vehicle actuator, constraint shaping is performed on the steering control command in the first control command used for path tracking.

10. The trajectory planning method for a mixed track and non-track area as described in claim 9, characterized in that, The path tracking endpoint state achieved by the first control command after constraint shaping, and the starting state of the non-track segment smooth trajectory planned by the second control command, satisfy a preset continuity rule, including: Based on the maximum acceleration constraint of the automated guided vehicle actuator, constraint shaping is performed on the speed control command in the first control command used for path tracking; The constrained steering control command and the constrained speed control command are combined to obtain the constrained first control command; The actual state of the automated guided vehicle at the connection point is obtained, including the actual position, actual heading angle and actual curvature. Determine whether the actual state and the initial reference value satisfy a preset continuity rule; the continuity rule requires that the distance error between the actual position and the position in the initial reference value is less than a first error threshold, the difference between the actual heading angle and the heading angle in the initial reference value is less than a second error threshold, and the difference between the actual curvature and the curvature in the initial reference value is less than a third error threshold. If the continuity rule is satisfied, the path tracking endpoint state implemented by the first control command after constraint shaping is determined to be continuous with the starting state of the non-track segment smooth trajectory planned by the second control command.

Images