Hybrid local planning method and system based on dynamic state diagram

Abstract

The invention provides a hybrid local planning method and system based on a dynamic state diagram, and the method comprises the steps: calculating a displacement distance range and an orientation angle range of a target point based on the operation parameters, obtained in real time, of an autonomous mobile robot and a pre-constructed initial dynamic diagram; calculating state nodes based on the target point displacement distance range, the orientation angle range and the initial dynamic graph, and constructing a dynamic state graph through the state nodes; constructing a target path based on the dynamic state diagram and a preset search algorithm to obtain a target point, and obtaining a target linear speed and a target point orientation according to the target point and a preset speed strategy; obtaining a target angular velocity based on the target point and a preset path planning algorithm; and sending the target linear velocity, the target angular velocity and the target point orientation to the autonomous mobile robot, so that the autonomous mobile robot plans a local path. According to the method, the optimal path is dynamically and locally adjusted in real time under kinematics constraints, and the path planning reliability is improved.

Description

Technical Field

[0001] This invention relates to the field of path planning, and more particularly to a hybrid local planning method and system based on dynamic state diagrams. Background Technology

[0002] Path planning is one of the core capabilities of autonomous mobile robots. Its core task is to enable the robot to move autonomously from the starting point to the destination without human intervention. Path planning can be divided into two levels: global planning and local planning. Global planning is relatively mature and there are various applicable algorithms to achieve optimal path selection. However, local planning has the problem of nondeterministic polynomial time complexity, which is difficult to optimize.

[0003] Currently, existing technologies addressing the problems of local path planning include the Dynamic Window Approach (DWA), the Elastic Band (EBand) local planning method, and artificial neural networks for local path planning. DWA achieves obstacle avoidance through velocity sampling and short-term trajectory prediction; however, it is prone to path oscillations in scenarios with dense dynamic obstacles. The Elastic Band local planning method employs a dual mechanism of contraction and repulsion to achieve dynamic obstacle avoidance while maintaining global path tracking; however, its fixed weight mechanism for contraction and repulsion makes it difficult to dynamically coordinate global path tracking and local obstacle avoidance. Artificial neural networks for local path planning require a large amount of training data, have insufficient generalization ability, and struggle to adapt to rapidly changing dynamic environments. Therefore, existing technologies struggle to balance response speed and path quality in complex dynamic environments, resulting in poor path planning reliability. Summary of the Invention

[0004] To address the aforementioned issues, this invention proposes a hybrid local planning method and system based on dynamic state diagrams, which enables real-time dynamic local adjustment of the optimal path under kinematic constraints, thereby improving the reliability of path planning.

[0005] To achieve the above objectives, embodiments of the present invention provide a hybrid local planning method based on a dynamic state diagram, comprising: calculating the displacement distance range and orientation angle range of a target point based on real-time acquired autonomous mobile robot operating parameters and a pre-constructed initial dynamic diagram; calculating state nodes based on the displacement distance range, orientation angle range, and the initial dynamic diagram, and constructing a dynamic state diagram through the state nodes; constructing a target path based on the dynamic state diagram and a preset search algorithm to obtain a target point, and obtaining the target linear velocity and target point orientation according to the target point and a preset velocity strategy; obtaining the target angular velocity based on the target point and a preset path planning algorithm; and sending the target linear velocity, target angular velocity, and target point orientation to the autonomous mobile robot so that the autonomous mobile robot can plan a local path.

[0006] This invention proposes a hybrid local planning method based on a dynamic state diagram. It acquires the operating parameters of an autonomous mobile robot in real time, calculates the displacement distance range and orientation angle range of a target point in a pre-constructed initial dynamic diagram, and constructs a dynamic state diagram. This dynamic state diagram allows the robot to adapt to dynamic environmental changes. A preset search algorithm is then used to construct the target path and acquire the target point within the dynamic state diagram, ensuring path quality while significantly reducing the risk of path oscillation. Next, a preset speed strategy is used to calculate the target linear velocity and target point orientation, and a preset path planning algorithm is used to calculate the target angular velocity. Finally, the target linear velocity, target angular velocity, and target point orientation are sent to the autonomous mobile robot to enable it to plan a local path. Thus, the construction of the dynamic state diagram ensures global path optimization, the preset speed strategy achieves real-time adaptability of linear velocity, and the preset path planning algorithm ensures the kinematic feasibility of angular velocity. These three aspects are integrated through a hierarchical decision-making architecture, enabling the autonomous mobile robot to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. Furthermore, under kinematic constraints, the optimal path is dynamically adjusted in real time, improving the reliability of path planning.

[0007] This invention also proposes a hybrid local planning system based on a dynamic state diagram, comprising: a target point constraint range calculation module, a dynamic state diagram construction module, a target linear velocity and target point orientation calculation module, a target angular velocity calculation module, and a local path planning module. The target point constraint range calculation module calculates the target point displacement distance range and orientation angle range based on real-time acquired autonomous mobile robot operating parameters and a pre-constructed initial dynamic diagram. The dynamic state diagram construction module calculates state nodes based on the target point displacement distance range, orientation angle range, and the initial dynamic diagram, and constructs a dynamic state diagram using these state nodes. The target linear velocity and target point orientation calculation module constructs a target path based on the dynamic state diagram and a preset search algorithm to obtain the target point, and obtains the target linear velocity and target point orientation according to the target point and a preset velocity strategy. The target angular velocity calculation module obtains the target angular velocity based on the target point and a preset path planning algorithm. The local path planning module sends the target linear velocity, target angular velocity, and target point orientation to the autonomous mobile robot so that the autonomous mobile robot can plan a local path.

[0008] This invention proposes a hybrid local planning system based on a dynamic state diagram. A target point constraint range calculation module acquires the autonomous mobile robot's operating parameters in real time. Within a pre-constructed initial dynamic diagram, it calculates the target point's displacement distance range and orientation angle range in real time. A dynamic state diagram construction module then constructs the dynamic state diagram, enabling adaptation to dynamic environmental changes. A target linear velocity and target point orientation calculation module then constructs the target path and acquires the target point within the dynamic state diagram using a preset search algorithm, ensuring path quality while significantly reducing path oscillation risk. Finally, based on a preset velocity strategy, the target linear velocity and target point orientation are calculated, along with the target angular velocity... The calculation module calculates the target angular velocity using a preset path planning algorithm. Finally, the local path planning module sends the target linear velocity, target angular velocity, and target point orientation to the autonomous mobile robot so that the autonomous mobile robot can plan a local path. Thus, by constructing a dynamic state graph to ensure the global optimality of the path, the preset velocity strategy to achieve real-time adaptability of the linear velocity, and the preset path planning algorithm to ensure the kinematic feasibility of the angular velocity, the three are integrated through a hierarchical decision architecture. This enables the autonomous mobile robot to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments, and then dynamically adjust the optimal path in real time under kinematic constraints, thereby improving the reliability of path planning. Attached Figure Description

[0009] Figure 1 A flowchart illustrating the steps of a hybrid local planning method based on a dynamic state diagram, provided in a certain embodiment of the present invention;

[0010] Figure 2 This is a schematic diagram illustrating the target point displacement range of a hybrid local planning method based on a dynamic state diagram, provided in a certain embodiment of the present invention.

[0011] Figure 3 A schematic diagram of the orientation angle range of a hybrid local planning method based on a dynamic state diagram provided in a certain embodiment of the present invention;

[0012] Figure 4 A tree structure diagram illustrating a hybrid local planning method based on a dynamic state diagram provided in a certain embodiment of the present invention;

[0013] Figure 5 A schematic diagram of a dynamic state diagram for a hybrid local planning method based on a dynamic state diagram provided in a certain embodiment of the present invention;

[0014] Figure 6 A schematic diagram of the target path search results of a hybrid local planning method based on a dynamic state diagram provided in a certain embodiment of the present invention;

[0015] Figure 7This is a schematic diagram of the module structure of a hybrid local planning system based on a dynamic state diagram, provided in a certain embodiment of the present invention. Detailed Implementation

[0016] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0017] Example 1

[0018] See Figure 1 , Figure 1 This is a flowchart illustrating the steps of a hybrid local planning method based on a dynamic state diagram, provided in one embodiment of the present invention. Figure 1 As shown, this embodiment of the invention proposes a hybrid local planning method based on a dynamic state diagram, including steps 101 to 105, each step being as follows: Step 101, based on the real-time acquired autonomous mobile robot operating parameters and a pre-constructed initial dynamic diagram, calculate the target point displacement distance range and orientation angle range; Step 102, based on the target point displacement distance range, orientation angle range, and the initial dynamic diagram, calculate state nodes, and construct a dynamic state diagram through the state nodes; Step 103, based on the dynamic state diagram and a preset search algorithm, construct a target path to obtain the target point, and based on the target point and a preset speed strategy, obtain the target linear velocity and target point orientation; Step 104, based on the target point and a preset path planning algorithm, obtain the target angular velocity; Step 105, send the target linear velocity, target angular velocity, and target point orientation to the autonomous mobile robot so that the autonomous mobile robot can plan a local path.

[0019] One specific implementation involves acquiring processed autonomous mobile robot operating parameters in real time from a lidar, visual data processor, and map information processor mounted on the autonomous mobile robot. These parameters include the robot's current velocity, current maximum acceleration, current angular velocity, and current maximum angular acceleration. In this embodiment, a pre-constructed initial dynamic graph is set to include L=20 decision layers, with each decision layer containing N=60 initial nodes. The total number of nodes, including the start and end points, is represented as L*N+2. The target point's displacement distance range and orientation angle range are calculated based on the autonomous mobile robot's operating parameters and the initial dynamic graph. Within these ranges, the state information of each initial node is calculated, including its position and orientation angle. This state information is then combined with the initial node's state information to form a tree structure graph, expanding layer by layer. Edges connect the initial nodes at each layer within the tree structure graph, resulting in a dynamic state graph. The edge cost of each node is then calculated. A preset search algorithm is used to select the path with the minimum edge cost as the target path and search for the target point. The preset search algorithm can be Dijkstra's algorithm. Then, the target linear velocity is calculated using a preset speed strategy that includes two stages: "target point reachability judgment" and "obstacle cost adaptive adjustment." Differentiated speed penalties are provided for different obstacle densities. A local search interval is then formed centered on the target point. A candidate orientation angle set is generated by discretizing the angle of the local search interval, and the target point orientation is further selected. Finally, a preset path planning algorithm is used to extend the state in three-dimensional space. By discretizing the spatial resolution, computational efficiency and path accuracy are balanced. The path search state diagram is constructed by further extending the planning nodes, and the cost value of each planning node within a preset simulation period is calculated. The optimal planning path is selected, and the target angular velocity of each planning node on the optimal planning path is calculated. The preset path planning algorithm can adopt the HybridA* algorithm. The target linear velocity, target angular velocity, and target point orientation are converted into corresponding control commands and sent to the execution layer of the autonomous mobile robot. The Ackerman chassis actuator installed inside the autonomous mobile robot plans the local path.

[0020] This invention proposes a hybrid local planning method based on a dynamic state diagram. It acquires the operating parameters of an autonomous mobile robot in real time, calculates the displacement distance range and orientation angle range of a target point in a pre-constructed initial dynamic diagram, and constructs a dynamic state diagram. This dynamic state diagram allows the robot to adapt to dynamic environmental changes. A preset search algorithm is then used to construct the target path and acquire the target point within the dynamic state diagram, ensuring path quality while significantly reducing the risk of path oscillation. Next, a preset speed strategy is used to calculate the target linear velocity and target point orientation, and a preset path planning algorithm is used to calculate the target angular velocity. Finally, the target linear velocity, target angular velocity, and target point orientation are sent to the autonomous mobile robot to enable it to plan a local path. Thus, the construction of the dynamic state diagram ensures global path optimization, the preset speed strategy achieves real-time adaptability of linear velocity, and the preset path planning algorithm ensures the kinematic feasibility of angular velocity. These three aspects are integrated through a hierarchical decision-making architecture, enabling the autonomous mobile robot to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. Furthermore, under kinematic constraints, the optimal path is dynamically adjusted in real time, improving the reliability of path planning.

[0021] A preferred embodiment calculates the target point displacement distance range and orientation angle range based on real-time acquired autonomous mobile robot operating parameters and a pre-constructed initial dynamic graph. This includes: acquiring the autonomous mobile robot's operating parameters in real-time to obtain its current velocity, current maximum acceleration, current angular velocity, and current maximum angular acceleration; constructing an initial dynamic graph, which includes several decision layers and several initial nodes; calculating braking distance and predicted distance values ​​based on the current velocity and current maximum acceleration, and obtaining the target point displacement distance range and the decision layer spacing based on these values; and calculating the actual angle change range of the autonomous mobile robot based on the current angular velocity and current maximum angular acceleration, and obtaining the orientation angle range and the angle difference between adjacent initial nodes based on this range. One preferred implementation involves acquiring processed autonomous mobile robot operating parameters in real time from a lidar, visual data processor, and map information processor mounted on the autonomous mobile robot. These parameters include the robot's current velocity, current maximum acceleration, current angular velocity, and current maximum angular acceleration. In this embodiment, a pre-constructed initial dynamic graph is configured with L = 20 decision layers, each with N = 60 initial nodes. The total number of nodes, including the start and end points, is represented as L*N+2. The target point's displacement distance range and orientation angle range are calculated based on the autonomous mobile robot's operating parameters and the initial dynamic graph. Specifically, in the pre-constructed initial dynamic graph, the combination of decision layers and initial node numbers has been verified by Monte Carlo simulation to complete the calculation within a 0.05s planning cycle, demonstrating high computational efficiency. For the calculation of the target point's displacement distance range, see [link to relevant documentation]. Figure 2 , Figure 2 This is a schematic diagram illustrating the target point displacement range of a hybrid local planning method based on a dynamic state diagram, provided in a certain embodiment of the present invention; as shown below. Figure 2 As shown, the braking distance value represents the braking distance considering the current speed and the current maximum acceleration. Figure 2 The middle is represented as d min The specific calculation formula is as follows:

[0022]

[0023] In the formula, t acc To account for the response latency of the servo system and ensure the minimum braking time for safety during emergency braking, t acc =min(Δt,t) amax- ), Δt is the control period, t amax- This is the time required for the current speed to decelerate to the minimum speed limit. v cur v is the current speed. min For autonomous mobile robots, a minimum speed limit is allowed. max This represents the current maximum acceleration.

[0024] The predicted distance value is calculated over a preset fixed prediction time T. cap =2s, the maximum distance that an autonomous mobile robot can reach, in Figure 2 The middle is represented as d max The specific calculation formula is as follows:

[0025] V1 = v max ·(T cap -t amax+ );

[0026]

[0027] V3=(v cur +a max ·Δt)·(T cap -Δt);

[0028]

[0029] In the formula, t amax+ The time required to accelerate from the current speed to the maximum speed limit. V1 represents the distance the autonomous mobile robot can travel at its maximum speed within one control cycle, indicating good performance and the ability to reach the maximum speed relatively quickly within the predicted time. V2 represents the minimum distance the autonomous mobile robot needs to travel to reach its maximum speed within one control cycle, indicating good performance and the ability to accelerate from its current speed. V3 represents the distance the autonomous mobile robot can travel at its actual maximum speed within the remaining time, indicating poor performance and the inability to reach the maximum speed allowed by its performance within the predicted time. V4 represents the minimum distance the autonomous mobile robot needs to travel at its actual maximum speed, indicating poor performance and the inability to reach the maximum speed allowed by its performance within the predicted time. After calculating the braking distance and predicted distance, the following can be obtained: Figure 2 The target point displacement range shown is further analyzed by subtracting the braking distance value from the predicted distance value to obtain the decision layer spacing. Figure 2 The orientation angle range is represented by Δd. The orientation angle range of an autonomous mobile robot can be characterized as its steering capability. However, the steering capability of an autonomous mobile robot is limited by the current angular velocity and the maximum angular acceleration. Therefore, the calculation of the orientation angle range needs to dynamically reflect the physical constraints of the steering system. By distinguishing between two working conditions, acceleration steering and uniform steering, the maximum angle boundaries for left and right steering are constructed respectively to ensure that the searched orientation angle is always within the range that the robot can actually achieve. In this embodiment, all angles are normalized and limited to the range of [0, 2π] to avoid ambiguity in the calculation of periodic angles and to improve the geometric consistency of the subsequent dynamic state diagram construction.

[0030] In the above scheme, real-time acquired robot operating parameters are used to dynamically determine the target point displacement range and decision-layer spacing by calculating braking distance and predicted distance. This ensures that path planning strictly conforms to kinematic constraints, avoiding path infeasibility or collision risks caused by sudden velocity changes. Simultaneously, based on real-time calculation of the actual angle change range using angular velocity and angular acceleration, the orientation angle range and the angle difference between adjacent nodes are precisely limited, effectively suppressing path oscillations caused by steering overshoot. Combined with a pre-constructed initial dynamic graph, the search space is dynamically constrained, enabling the autonomous mobile robot to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. Furthermore, under kinematic constraints, the optimal path is dynamically adjusted locally in real-time, improving the reliability of path planning.

[0031] A preferred embodiment calculates braking distance and predicted distance based on current speed and current maximum acceleration, and obtains the target point displacement range and decision layer spacing based on the braking distance and predicted distance. This includes: calculating the first time required for the current speed to decelerate to a preset first speed threshold using the current speed and current maximum acceleration, and calculating the braking time based on the first time and a preset control cycle; calculating the braking distance using the current speed, current maximum acceleration, and braking time; calculating the second time required for the current speed to accelerate to a preset second speed threshold using the current speed and current maximum acceleration; calculating the predicted distance using the current speed, current maximum acceleration, preset predicted time, second time, and preset control cycle; and calculating the decision layer spacing using the braking distance, predicted distance, and number of decision layers. Another preferred embodiment, for calculating the target point displacement range, see [link to implementation details]. Figure 2 , Figure 2 This is a schematic diagram illustrating the target point displacement range of a hybrid local planning method based on a dynamic state diagram, provided in a certain embodiment of the present invention; as shown below. Figure 2 As shown, the braking distance value represents the braking distance considering the current speed and the current maximum acceleration. Figure 2 The middle is represented as d min The specific calculation formula is as follows:

[0032]

[0033] In the formula, t acc To account for the response latency of the servo system and ensure the minimum braking time for safety during emergency braking, t acc =min(Δt,t) amax- ), Δt is the control period, t amax- This is the time required for the current speed to decelerate to the minimum speed limit (equivalent to the first instant). v cur v is the current speed. min For autonomous mobile robots, a minimum speed limit is allowed. max This represents the current maximum acceleration.

[0034] The predicted distance value is calculated over a preset fixed prediction time T. cap =2s, the maximum distance that an autonomous mobile robot can reach, in Figure 2 The middle is represented as d max The specific calculation formula is as follows:

[0035] V1 = v max ·(T cap -t amax+ );

[0036]

[0037] V3=(v cur +a max ·Δt)·(T cap -Δt);

[0038]

[0039] In the formula, t amax+ This is the time required to accelerate from the current speed to the maximum speed limit (equivalent to the second time). v max The maximum speed limits for autonomous mobile robots are defined as follows: V1 represents the distance traveled at maximum speed within a control cycle, indicating good performance and the ability to reach maximum speed relatively quickly within the predicted time; V2 represents the minimum distance traveled to reach maximum speed within a control cycle, indicating good performance and the ability to accelerate from the current speed to the maximum speed; V3 represents the distance traveled within the remaining time at the actual maximum speed achievable by the autonomous mobile robot, indicating poor performance and the inability to reach the maximum speed allowed by its performance within the predicted time; and V4 represents the minimum distance traveled at the actual maximum speed achieved by the autonomous mobile robot, indicating poor performance and the inability to reach the maximum speed allowed by its performance within the predicted time.

[0040] After calculating the braking distance and predicted distance, the following can be obtained: Figure 2 The target point displacement range shown is further analyzed by subtracting the braking distance value from the predicted distance value to obtain the decision layer spacing. Figure 2 The value is represented as Δd, and the specific calculation method is as follows:

[0041]

[0042] Based on the calculation of the target point displacement range, actual scene testing shows that it can cover more than 90% of the dynamic obstacle motion prediction needs.

[0043] In the above scheme, the braking distance and predicted distance are dynamically calculated based on the current speed and maximum acceleration, precisely constraining the target point displacement range. The braking distance is calculated using deceleration time thresholds and braking time to ensure that path planning always meets the physical feasibility of emergency obstacle avoidance, avoiding path infeasibility or collision risks caused by sudden speed changes. The maximum achievable distance is dynamically calculated using fixed preset prediction time and acceleration limits, ensuring that the displacement range covers more than 90% of the dynamic obstacle trajectory, improving path foresight. Finally, the decision layer spacing is calculated using braking distance, predicted distance, and the number of decision layers, compressing the search space under kinematic constraints and reducing redundant node calculations. Thus, the dual physical constraints of calculating braking safety boundaries based on displacement range and predicting dynamic obstacles ensure path feasibility from the source, while optimizing the layer spacing improves the efficiency of state graph construction. Both work together to resolve the contradiction between response speed and path quality in dynamic environments, enabling autonomous mobile robots to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. Furthermore, under kinematic constraints, the optimal path is dynamically and locally adjusted in real time, improving the reliability of path planning.

[0044] A preferred embodiment calculates the actual angle change range of the autonomous mobile robot based on the current angular velocity and the current maximum angular acceleration, and obtains the orientation angle range and the angle difference between adjacent initial nodes based on the actual angle change range of the autonomous mobile robot. This includes: calculating the left and right turning angle ranges of each decision layer using the current angular velocity and the current maximum angular acceleration, and calculating the angle change range of each decision layer based on the number of decision layers; calculating the actual angle change range of the autonomous mobile robot based on the left and right turning angle ranges of each decision layer; calculating the orientation angle range based on the angle change ranges of each decision layer and the actual angle change range of the autonomous mobile robot; and calculating the angle difference between adjacent initial nodes based on the orientation angle range and the number of initial nodes. In one possible implementation of this preferred embodiment, the orientation angle range of the autonomous mobile robot can characterize its steering capability. Since the steering capability of the autonomous mobile robot is limited by the current angular velocity and the maximum angular acceleration, the calculation of the orientation angle range needs to dynamically reflect the physical constraints of the steering system by distinguishing between acceleration steering (t...). l_max <t lay ) and uniform turning (t) l_max ≥t lay For both working conditions, maximum angle boundaries for left and right turns are constructed to ensure that the search orientation angle is always within the actual achievable range of the robot. In this embodiment, all angles are normalized and limited to the range [0, 2π] to avoid ambiguity in the calculation of periodic angles and to improve the geometric consistency of subsequent dynamic state diagram construction. For a more detailed explanation, see [link to explanation]. Figure 3 , Figure 3 A schematic diagram illustrating the orientation angle range of a hybrid local planning method based on a dynamic state diagram, provided in a certain embodiment of the present invention; as shown. Figure 3As shown, the angular range of each decision layer is determined by the current angular velocity and the current maximum angular acceleration. The specific calculation formula for the left and right turning angle range of each decision layer is as follows:

[0045]

[0046] θ l1 =ω cur ·t l_max θ r1 =ω cur ·t r_max ;

[0047]

[0048] θ l3 =ω max ·(t lay -t l_max );

[0049] θ r3 =ω max ·(t lay -t r_max );

[0050] θ lay1 =ω cur ·t lay ;

[0051]

[0052] In the formula, t represents the time t taken for each decision-making level to turn. lay = (lay+1)·Δt, where lay is the floor number; t l_max The time required to reach maximum angular velocity when turning left; t r_max The time required to reach maximum angular velocity when turning right; ω max Maximum allowable angular velocity limit for the robot; ω cur α is the current angular velocity; max θ represents the current maximum angular acceleration. l1 The left-turn angle θ is the time required to reach the maximum angular velocity at the current angular velocity. r1 The right turn angle is the time required to reach the maximum angular velocity at the current angular velocity; θ is the angle of right turn. l2 To determine the maximum angular velocity during a left turn, if angular acceleration exists, then θ l2 The left turn angle is obtained from the angular acceleration component; θ r2 To determine the maximum angular velocity during a right turn, if angular acceleration exists, then θ r2 This is the right turn angle obtained from the angular acceleration component. Since the left and right turn directions are different, the formula uses a positive or negative sign; θ l3If the preset prediction time is greater than the time required to turn left to the maximum angle, it means the robot has sufficient time to turn left to the maximum angular velocity within one cycle, and the extra time will be used to continue turning left at the maximum angular velocity; θ r3 If the preset, fixed prediction time is greater than the time required to turn right to the maximum angle, it means the robot has sufficient time to turn right to the maximum angular velocity within one cycle, and the extra time will be used to continue turning right at the maximum angular velocity; θ lay1 and θ lay2 It is an intermediate quantity in the calculation process, with no special meaning, used only to simplify the calculation formula; θ l_max θ represents the theoretically maximum left turn angle achievable by each layer of the robot. r_max This represents the maximum right turn angle that each layer of the robot can theoretically achieve.

[0053] Further, the angle variation range for each decision-making level is calculated based on the number of decision-making levels. This typically involves calculating the maximum permissible angle variation range for each decision-making level, as detailed below:

[0054]

[0055] And calculate the actual angle change range of the autonomous mobile robot based on the left and right turning angle range of each decision layer. Usually, it is to calculate the maximum angle change range that the autonomous mobile robot can actually achieve at the current angle, as follows:

[0056] Δθ tru =min(|θ l_max -θ r_max |,min(|θ l_max -θ r_max -2π|,|θ l_max -θ r_max +2π|));

[0057] Finally, by comparing the maximum allowable angle change range of each decision layer with the maximum actual angle change range that the autonomous mobile robot can achieve at the current angle, the maximum left and right turning angle θ of the autonomous mobile robot is determined. tru_l_max and θ tru_r_max , representing the range of orientation angles, is calculated using the following formula:

[0058]

[0059] Furthermore, the angle difference between adjacent initial nodes in each decision layer needs to be calculated for the subsequent construction of the dynamic state diagram. The specific calculation formula is as follows:

[0060]

[0061] In the above scheme, the left and right turning angle ranges of each decision layer are dynamically calculated based on the current angular velocity and maximum angular acceleration, and the theoretical angle change range is generated by integrating the number of decision layers. By comparing the theoretical range with the actual physically achievable angle range, the orientation angle search space is accurately compressed, eliminating dynamically infeasible turning actions. Based on this compressed orientation angle range, the angle difference between adjacent nodes is generated by uniformly discretizing the initial number of nodes, thus constraining the degree of freedom from the kinematic source. This avoids path oscillation and reduces the expansion of invalid nodes, improving the efficiency of state graph construction while ensuring smooth turning. This solves the trade-off between path quality and response speed in dynamic environments, enabling autonomous mobile robots to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. Furthermore, under kinematic constraints, the optimal path is dynamically and locally adjusted in real time, improving the reliability of path planning.

[0062] A preferred scheme involves calculating state nodes based on the target point displacement distance range, orientation angle range, and an initial dynamic graph, and constructing a dynamic state graph using these state nodes. This includes: calculating the state information of initial nodes in each decision layer of the initial dynamic graph using the target point displacement distance range and orientation angle range, obtaining state nodes, where the state information includes node position and node orientation angle; converting the initial dynamic graph into a tree structure graph based on the state nodes; setting the same index value for all state nodes in the same decision layer of the tree structure graph; taking the current position of the autonomous mobile robot as the starting point, if the index value of the current decision layer is less than or equal to a preset threshold, calculating an edge connecting each state node in the current decision layer to an adjacent state node in the previous decision layer; if the index value of the current decision layer is greater than the preset threshold, calculating n edges connecting each state node in the current decision layer to n adjacent state nodes in the previous decision layer, where if a state node is at either end of a decision layer, calculating n-1 edges connecting the state node to n-1 adjacent state nodes in the previous decision layer; traversing the index values, and obtaining the dynamic state graph after all decision layers corresponding to each index value have been connected. One preferred implementation involves calculating the state information of each initial node within the target point's displacement distance and orientation angle range. This state information includes the node's position and orientation angle. Combining this state information with the initial node's state information, a tree-like structure is formed layer by layer. The initial nodes at each layer are connected by edges within the tree-like structure to obtain a dynamic state diagram. For a more detailed explanation, see [link to explanation]. Figure 4 , Figure 4 This is a schematic diagram of a tree structure of a hybrid local programming method based on a dynamic state diagram, provided in a certain embodiment of the present invention; as shown below. Figure 4 As shown, for each initial node, the initial node is denoted as p. i Their status information all include position (x) i ,y i ) and orientation angle θ ncurThe specific calculation formula for calculating the state information of each initial node layer by layer is as follows:

[0063]

[0064] In the formula, x i y i and z i These are the three components of the next node, x cur and y cur d represents the x and y components of the current node. ncur θ represents the base distance from the current node to the next node. ncur It is the basic turning angle from the current node to the next node, i.e., the orientation angle, for the initial layer d. ncur =d min For each initial point on a given level, the base turning angle is equal to the maximum actual left turn angle of that level, i.e., θ. ncur =θ tru_l_max For the creation of a new layer, there is d ncur =d ncur +Δd, create new nodes in the same layer, and the angle θ of each new node. new There is θ new =θ ncur -Δθ nodes ;

[0065] After calculating the state information of each initial node, the corresponding state nodes are obtained. Then, by expanding layer by layer, the initial dynamic graph is transformed into something like... Figure 4 The tree structure diagram shown assigns an index value to each decision level, and all state nodes within the same decision level have the same index value. For example, starting from the current position of the autonomous mobile robot, the index value is set to 0. The 0th level of the tree structure diagram has only one node, with an index value of 1. Subsequent levels each have N state nodes. In this embodiment, a preset threshold is set to 1. If the index value of the current decision level is less than or equal to the preset threshold (i.e., the current index value is less than or equal to 1), since the 0th level has only one node, each node in the 1st level of the tree structure diagram only needs to calculate one edge connecting it to the adjacent node in the previous level. If the index value of the current decision level is greater than the preset threshold (i.e., the current index value is greater than 1), then each state node in the current decision level has n edges connecting it to the n adjacent state nodes in the previous decision level. If a state node is at either end of the decision level, then n-1 edges connecting it to the n-1 adjacent state nodes in the previous decision level are calculated. Specifically, see [link to relevant documentation]. Figure 5 , Figure 5 A schematic diagram of a dynamic state diagram-based hybrid local programming method according to a certain embodiment of the present invention is provided; for example... Figure 5As shown, in the tree structure graph, each state node in the first level is connected to a node in the 0th level by only one edge. Each state node in the second level is connected to three adjacent state nodes in the first level. Each state node in the second level, except for the two nodes at the very end, is connected to the first level by three edges. The two state nodes at the very end only need to be connected to the two adjacent state nodes in the first level, i.e., connected by two edges. When all index values ​​are traversed, it is found that all index values ​​are connected by edges, thus forming a tree structure as shown below. Figure 5 The dynamic state diagram shown.

[0066] In the above scheme, a hierarchical connection rule is used to compress the search space, while the tree topology ensures path reachability; the physical constraints of node states and edge connections eliminate the risk of path oscillation from the source, so that the dynamic state graph has both efficient construction and high-quality path generation capabilities, significantly improving the planning reliability in complex environments.

[0067] A preferred embodiment involves constructing a target path based on a dynamic state graph and a preset search algorithm to obtain a target point, and obtaining the target linear velocity and target point orientation based on the target point and a preset speed strategy. This includes: calculating the edge cost of each edge in the dynamic state graph using a preset node cost function; selecting a target path composed of several edges whose current edge cost satisfies the preset path requirements using a priority queue based on the preset search algorithm, and obtaining the target point based on the target path; calculating a first distance from the current position of the autonomous mobile robot to the target point and a second distance traveled by the autonomous mobile robot within a preset target point decision time window; if the first distance is less than or equal to the second distance, using the first distance as the allowable distance, calculating a first allowable linear velocity based on the quotient of the first distance and the preset target point decision time window, and using the first allowable linear velocity as the target linear velocity and the target point orientation angle as the target point orientation; if the first distance is greater than the second distance, calculating the actual allowable linear velocity according to the preset speed strategy, and using the actual allowable linear velocity as the target linear velocity; and discretizing the target point orientation angle to generate a candidate orientation angle set and obtaining an orientation angle that satisfies the preset alignment cost requirements from the candidate orientation angle set using a preset angle filtering principle as the target point orientation. One preferred implementation involves calculating the edge cost of each node, selecting the path formed by the edge with the minimum edge cost as the target path using a preset search algorithm, and then searching for the target point. The preset search algorithm can be Dijkstra's algorithm. Specifically, after constructing the dynamic state graph, the edge cost of each edge in the dynamic state graph is calculated using a preset node cost function. Firstly, the node cost in the node cost function includes: the comprehensive obstacle cost C. obs Path deviation cost C path Cost C to get close to the target goal By setting the weight coefficient ω corresponding to the cost of each node obs ω path and ωgoa The node cost can be dynamically adjusted according to the scenario. node The specific calculation formula is as follows:

[0068] C node =ω obs ·C obs +ω path ·C path +ω goal ·C goal ;

[0069] Then, combining the Euclidean example penalty between adjacent state nodes, the edge cost C of each edge is calculated. edge The specific calculation formula is as follows:

[0070]

[0071] In the formula, d cur_i Let d be the Euclidean distance between adjacent state nodes in the current decision layer. prv_i Let x be the Euclidean distance between adjacent state nodes in the previous decision layer, where i is the node index and x is the distance between adjacent state nodes. prv and y prv These are the coordinate components of the previous layer node, and λ is the penalty weight value. See also Figure 6 , Figure 6 This is a schematic diagram illustrating the target path search results of a hybrid local planning method based on a dynamic state diagram, provided in a certain embodiment of the present invention; for example... Figure 6 As shown, Dijkstra's algorithm is finally used to select the path formed by the edges with the minimum cost (equivalent to the preset path requirement) using a priority queue. The node costs and parent nodes are dynamically updated to obtain the target path, and the target point is searched within that path. The target path is as follows: Figure 6 The red text illustrates the process. Then, a preset speed strategy, including two stages—"target point reachability judgment" and "obstacle cost adaptive adjustment"—is used to calculate the target linear velocity. Differentiated speed penalties are provided for different obstacle densities. A local search interval is then formed centered on the target point. This local search interval is discretized to generate a set of candidate orientation angles, which are further filtered to obtain the target point orientation. Specifically, the target point orientation is first determined through a preset target point decision time window t. dec = Maximum travel distance d within 2 seconds dec (Equivalent to the second distance) Determine if the target point is within a controllable range: If the target point is close to d dec If the target point is far away, the speed will be scaled proportionally to ensure a smooth arrival; if the target point is far away, the speed will be scaled based on the maximum obstacle cost C in the path. obs_max_path Dynamically adjust the permissible speed. Based on the two scenarios above, calculate the permissible linear speed and permissible distance. Specifically, the distance from the current position to the target point (equivalent to the first distance) is calculated as follows:

[0072]

[0073] In the formula, x cur and y cur It is the component at the current position; x goal and y goal It is the component of the target point's position;

[0074] The maximum driving distance d within the preset target point decision time window dec The formula for calculating (equivalent to the second distance) is as follows:

[0075] d dec =v max ·t dec ;

[0076] In the formula, t dec This is a preset target point decision time window, which is set to 2 seconds in this embodiment; v max Maximum speed limits are allowed for autonomous mobile robots;

[0077] If the target point is within the farthest distance reachable within the preset target point decision time window, i.e., the first distance is less than or equal to the second distance, then the first allowable linear velocity and allowable distance are calculated as follows:

[0078] m = d goal / d dec ;

[0079] v allow =v max ·m;

[0080] d allow =d goal ;

[0081] In the formula, m is an intermediate parameter used to characterize v. allow With v max Relationship, v allow It is the maximum speed allowed within the target point decision time window (equivalent to the first allowed linear velocity), v max This is the maximum speed limit allowed for autonomous mobile robots;

[0082] Meanwhile, when the first distance is less than or equal to the second distance, the target point's orientation angle is taken as the target point's orientation, denoted as θ. head =θ goal .

[0083] If the target point is outside the maximum distance that can be reached within the preset target point decision time window, i.e., the first distance is greater than the second distance, then the actual allowable linear velocity is calculated according to the preset speed strategy, and the actual allowable linear velocity is used as the target linear velocity. In addition, the orientation angle of the target point is discretized to generate a set of candidate orientation angles, and the orientation angle that meets the preset alignment cost requirement is obtained from the set of candidate orientation angles through the preset angle screening principle as the orientation of the target point.

[0084] In the above scheme, in the dynamic state diagram, a preset search algorithm is driven by a preset node cost function to search for the target path, ensuring the global optimality of the path between obstacle avoidance, efficiency and target approach. Then, the first distance and the second distance distinguish between near-distance and far-distance scenarios. In different scenarios, a hierarchical speed strategy is adopted to respond synchronously to the dynamic changes of the environment. Discrete orientation search avoids the computational burden of continuous space. The three work together to achieve highly reliable dynamic path adjustment in complex scenarios, enabling the autonomous mobile robot to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. In turn, under kinematic constraints, the optimal path is dynamically adjusted locally in real time, improving the reliability of path planning.

[0085] In a preferred embodiment, if the first distance is greater than the second distance, the actual permissible linear velocity is calculated according to a preset speed strategy, and the actual permissible linear velocity is used as the target linear velocity. The process also includes: discretizing the orientation angle of the target point to generate a candidate orientation angle set; and selecting an orientation angle from the candidate orientation angle set that meets a preset alignment cost requirement as the target point orientation based on a preset angle selection principle. This includes: if the first distance is greater than the second distance, calculating the second permissible linear velocity based on the obstacle cost in the target path that meets the preset obstacle cost requirement; calculating the third time from the current speed to the second permissible speed under the current maximum acceleration, and obtaining the actual permissible linear velocity based on the third time, the current speed, the current maximum acceleration, and the second permissible linear velocity, and using the actual permissible linear velocity as the target linear velocity; discretizing the orientation angle of the target point according to a preset local search interval to obtain a candidate orientation angle set; calculating the orientation alignment cost value of each candidate orientation angle according to a preset angle selection principle, and selecting an orientation angle that meets the preset alignment cost requirement as the target point orientation. In one preferred implementation, if the target point is outside the maximum distance achievable within a preset target point decision time window (i.e., the first distance is greater than the second distance), then an appropriate speed needs to be selected based on a preset speed strategy. First, the maximum obstacle cost C within the map is calculated. obs_limit :

[0086] C obs_limit =254ω obs ;

[0087] Calculate the maximum obstacle cost C in the path (preset obstacle cost requirement). obs_max_pathCost C of the largest obstacle within the map obs_limit Relative relationship:

[0088]

[0089] Based on the maximum obstacle cost C on the path obs_max_path Calculate the maximum permissible linear velocity (equivalent to the second permissible velocity):

[0090] v allow =v max ·(1-f(s));

[0091] Wherein, the deceleration factor function f(s) is a piecewise linear function, and s is the deceleration factor:

[0092]

[0093] Among them, the deceleration factor function f(s) adopts a piecewise linear design, which provides differentiated speed penalties for different obstacle densities (quantified by normalized cost s). For example, when s≥0.75, the deceleration factor is significantly increased to cope with high-risk scenarios.

[0094] Finally, the maximum permissible linear velocity v is calculated, taking into account the actual computation time. final_allow (Equivalent to the actual permissible linear velocity), the specific calculation formula is as follows:

[0095]

[0096] In the formula, t sim To limit the calculation speed (i.e., the planning period, with the output frequency controlled at 20Hz, the time in this embodiment is 0.05s), t m The time required to reach the maximum permissible speed from the current speed under maximum acceleration (equivalent to the third time):

[0097]

[0098] After calculating the actual permissible linear velocity, the target point orientation is further calculated. A local search interval is formed by expanding π / 3 around the target point. This ensures both the tracking of the global path and reserves a turning margin for dynamic obstacle avoidance. A set of candidate orientation angles is generated by discretizing the local search interval, and the optimal solution is selected using the minimization principle to obtain the target point orientation. The specific explanation is as follows:

[0099] First, the orientation angle is divided, that is, the distance from the walking distance is calculated:

[0100]

[0101] In the formula, NUM_ANGLE represents the number of nodes in each layer; then, the search range for the target point's orientation angle is expanded to calculate the left and right boundaries:

[0102]

[0103] In the formula, θ goal The target point orientation angle is obtained from Dijkstra's algorithm; then the search range of the target point orientation angle is discretized:

[0104]

[0105] And calculate the discretized angle range dist and the number of iterations n:

[0106]

[0107] Therefore, a set of candidate orientation angles is constructed:

[0108]

[0109] Further compare the orientation alignment cost of each candidate angle, and select the orientation angle with the lowest cost (equivalent to the preset alignment cost requirement) as the target orientation angle. The specific calculation is as follows: For each candidate angle... Generate the trajectory and calculate the alignment cost as Cost. (k) Choose the angle that corresponds to the minimum cost:

[0110]

[0111] This gives us the orientation of the target point.

[0112] In the above scheme, in long-distance scenarios, the second permissible linear velocity is dynamically calculated through a piecewise deceleration function based on the maximum obstacle cost of the path, directly avoiding the risk of dynamic obstacle collisions from the kinematic source. Combining the current velocity, maximum acceleration, and the third time constraint required for acceleration or deceleration, a smooth transition of the actual permissible linear velocity is generated, eliminating path oscillations caused by sudden velocity changes. Simultaneously, the local search interval is expanded and discretized based on the target point's orientation angle, generating a finite set of candidate orientation angles. By calculating the trajectory alignment cost of each candidate angle and selecting the minimum value, the computational burden of continuous spatial search is avoided while ensuring smooth turning. The two mechanisms work synergistically: the velocity adaptive mechanism ensures real-time safety under sudden obstacles, while the discrete orientation optimization balances path tracking efficiency and computational real-time performance, enabling the autonomous mobile robot to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. Furthermore, under kinematic constraints, the optimal path is dynamically and locally adjusted in real-time, improving the reliability of path planning.

[0113] A preferred embodiment, based on a target point and a preset path planning algorithm, obtains the target angular velocity, including: constructing a path search state diagram according to the target point using the preset path planning algorithm, and obtaining a planned path from the path search state diagram within a preset simulation period; calculating the target angular velocity based on the orientation angle of the current planning node, the simulated orientation angle of the current node, the target linear velocity, and the target distance value, wherein if the first distance is less than or equal to the second distance, the target distance value is the first distance; if the first distance is greater than the second distance, the target distance value is the product of the actual allowed linear velocity and the preset target point decision time window. An alternative implementation of this preferred embodiment employs a preset path planning algorithm for state extension in three-dimensional space, balancing computational efficiency and path accuracy through discretized spatial resolution, wherein the preset path planning algorithm can be a HybridA* algorithm. Specifically, one explanation is that the boundary of the three-dimensional space is first calculated:

[0114]

[0115] Next, the space is discretized, that is, the space is divided, and the specific formula is as follows:

[0116]

[0117]

[0118] In the formula, NUM_X_DIVS represents the number of spatial discretizations in the x-direction, and NUM_Y_DIVS represents the number of spatial discretizations in the y-direction; both are 60 here. Further calculation of the simulation period is performed, where the prediction window t... cur_sim =1.5s and minimum sampling time t hybrid =d allow / 2 5 v sim_allow The combination of these factors ensures the real-time performance of the algorithm in high-speed motion scenarios. The specific calculation formula is as follows:

[0119] t cur_sim =1.5s;

[0120]

[0121] t allow =0.03s;

[0122] In the formula, t cur_sim t is the time of the preset prediction time window. hybrid t is the minimum sampling time within the preset prediction time window. allow The HybridA* algorithm calculates the time required for the current point to reach the nearest point, which is 0.03s in this case. In this embodiment, if the first distance is less than or equal to the second distance, the target distance is the first distance, i.e.:

[0123] d allow =d goal ;

[0124] If the first distance is greater than the second distance, then the target distance value is the product of the actual allowable linear velocity and the preset target point decision time window, that is:

[0125] d allow =v final_allow ·T cap ;

[0126] After discretizing the three-dimensional space, the path search state diagram is constructed by expanding the planning nodes, and the cost value of each planning node within the preset simulation period is calculated. The optimal planning path is then selected, and the target angular velocity of each planning node on the optimal planning path is calculated.

[0127] In the above scheme, a path search state graph is dynamically constructed based on the target point. An angular velocity calculation benchmark is determined through a distance adaptive strategy. The angular velocity is calculated in real time based on the target linear velocity and target distance value, taking into account the deviation between the current node orientation angle and the simulated orientation angle. This strictly satisfies kinematic constraints. Through adaptive switching of distance parameters, the steering control is kept consistent with path reachability and speed safety. Path oscillation and response delay are eliminated from the kinematic level, significantly improving the planning reliability in dynamic environments. This enables autonomous mobile robots to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. Furthermore, under kinematic constraints, the optimal path is dynamically and locally adjusted in real time, improving the reliability of path planning.

[0128] A preferred embodiment involves constructing a path search state graph based on a target point using a preset path planning algorithm, and obtaining a planned path from the path search state graph within a preset simulation period. This includes: constructing an extended region boundary and discretizing the spatial resolution based on the target point; expanding the planning nodes using a preset path planning algorithm based on a preset prediction time window to construct the path search state graph; calculating the cost value of each planning node within the preset simulation period based on the path search state graph, and selecting planning nodes that meet the preset path requirements through a priority queue to obtain the planned path. Another preferred embodiment involves further expanding the planning nodes after discretizing the three-dimensional space to construct the path search state graph, calculating the cost value of each planning node within the preset simulation period, selecting the optimal planned path, and calculating the target angular velocity of each planning node on the optimal planned path. Specifically, node expansion is similar to the construction process of a dynamic state graph, but simpler. The calculation formula is as follows:

[0129] θ l1 =ω cur ·tl_max θ r1 =ω cur ·t r_max ;

[0130]

[0131] θ l3 =ω max ·(t cur_sim -t l_max );

[0132] θ r3 =ω max ·(t lay -t r_max );

[0133] θ sim1 =ω cur ·t cur_sim ;

[0134]

[0135] In the formula, divs is the number of sampling points in the angle space, which is 30 here; t cur_sim The time frame for the preset prediction time window; θ l The maximum left turn angle used in the hybrid solution process; θ r The maximum right turn angle used in the hybrid solution process; θ dist The angular discrete interval used in the hybrid solution process, i.e., every θ dist There will be an angle sampling point, corresponding to an angle value; θ sim1 and θ sim2 It is an intermediate quantity in the calculation process, has no special meaning, and is only used to simplify the calculation formula;

[0136] After completing the planning node expansion, calculate the distance traveled within the current simulation cycle:

[0137] d cur =t cur_sim ·v sim_allow ;

[0138] In this embodiment, the minimum and maximum speeds allowed within the prediction period are:

[0139] v sim_min =max(v max ,v cur -a max ·t sim );

[0140] v sim_max =min(v max ,v cur+a max ·t sim );

[0141] The final linear velocity output based on the allowed velocity threshold is expressed as:

[0142]

[0143] Then calculate the status information of the newly planned nodes:

[0144]

[0145] And calculate the state information of the newly planned nodes after discretization:

[0146]

[0147] By expanding the planning nodes as described above, a path search state graph is constructed. The total cost of the newly planned nodes is calculated within this graph. The total cost includes: parent node movement cost, heuristic target approach cost, and obstacle cost. Unlike the node cost function, the heuristic function uses a weighted sum of Manhattan distance and angle difference to guide the search to prioritize expansion towards the target area. The specific calculation process is as follows:

[0148]

[0149] In the formula, Δθ parent Let Δx be the angle difference between the newly planned node and its parent node. parent The x-component of the displacement or distance difference between the newly planned node and its parent node; Δy parent C represents the y-component of the displacement or distance difference between the newly planned node and its parent node. parent C represents the movement cost between the newly planned node and its parent node. hr The cost of approaching the heuristic target for the new planning node, C cur The cost of the parent node, For the parent node heuristic cost, C obs For the obstacle cost of the newly planned node, C total The total cost is calculated using a priority queue to maintain nodes to be expanded. The minimum cost solution (equivalent to a preset path requirement) is selected based on the principle of minimum cost, and the optimal path (equivalent to a planned path) is generated by backtracking. Simultaneously, the angular velocity of each path point is calculated based on the current orientation deviation and velocity constraints to ensure the dynamic feasibility of the turning action and avoid the high-frequency oscillation problem common in traditional reactive algorithms. The specific calculation process is as follows:

[0150]

[0151] O = O dist +O sim ;

[0152] O dist =|O sim -O node_ori |;

[0153] In the formula, O node_ori It is the orientation angle of the current node, O dist The angle difference between the current node's orientation and the simulated orientation, where O is the target angle and k is the angular velocity threshold factor.

[0154] In the above scheme, the boundary of the extended region is dynamically constructed based on the target point. The search space is compressed by discretizing the spatial resolution to avoid the expansion of invalid nodes. A preset path planning algorithm is used to expand the planning nodes within a preset prediction time window. Combined with kinematic constraints, a physically feasible turning trajectory is generated. By driving the priority queue to select the optimal path through multi-cost comprehensive evaluation within a preset simulation period, the risk of path oscillation or collision caused by computation delay in traditional methods is eliminated. This enables the autonomous mobile robot to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments. Furthermore, under kinematic constraints, the optimal path is dynamically and locally adjusted in real time, thereby improving the reliability of path planning.

[0155] Example 2

[0156] See Figure 7 , Figure 7 This is a schematic diagram of the module structure of a hybrid local planning system based on a dynamic state diagram, provided in one embodiment of the present invention. Figure 7 As shown, this embodiment of the invention proposes a hybrid local planning system based on a dynamic state diagram, including: a target point constraint range calculation module 201, a dynamic state diagram construction module 202, a target linear velocity and target point orientation calculation module 203, a target angular velocity calculation module 204, and a local path planning module 205. The target point constraint range calculation module 201 calculates the target point displacement distance range and orientation angle range based on real-time acquired autonomous mobile robot operating parameters and a pre-constructed initial dynamic diagram. The dynamic state diagram construction module 202 calculates state nodes based on the target point displacement distance range, orientation angle range, and the initial dynamic diagram, and constructs a dynamic state diagram through the state nodes. The target linear velocity and target point orientation calculation module 203 constructs a target path based on the dynamic state diagram and a preset search algorithm to obtain the target point, and obtains the target linear velocity and target point orientation according to the target point and a preset velocity strategy. The target angular velocity calculation module 204 obtains the target angular velocity based on the target point and a preset path planning algorithm. The local path planning module 205 sends the target linear velocity, target angular velocity, and target point orientation to the autonomous mobile robot so that the autonomous mobile robot can plan a local path.

[0157] This invention proposes a hybrid local planning system based on a dynamic state diagram. A target point constraint range calculation module acquires the autonomous mobile robot's operating parameters in real time. Within a pre-constructed initial dynamic diagram, it calculates the target point's displacement distance range and orientation angle range in real time. A dynamic state diagram construction module then constructs the dynamic state diagram, enabling adaptation to dynamic environmental changes. A target linear velocity and target point orientation calculation module then constructs the target path and acquires the target point within the dynamic state diagram using a preset search algorithm, ensuring path quality while significantly reducing path oscillation risk. Finally, based on a preset velocity strategy, the target linear velocity and target point orientation are calculated, along with the target angular velocity... The calculation module calculates the target angular velocity using a preset path planning algorithm. Finally, the local path planning module sends the target linear velocity, target angular velocity, and target point orientation to the autonomous mobile robot so that the autonomous mobile robot can plan a local path. Thus, by constructing a dynamic state graph to ensure the global optimality of the path, the preset velocity strategy to achieve real-time adaptability of the linear velocity, and the preset path planning algorithm to ensure the kinematic feasibility of the angular velocity, the three are integrated through a hierarchical decision architecture. This enables the autonomous mobile robot to simultaneously optimize obstacle avoidance response speed and path tracking quality in complex dynamic environments, and then dynamically adjust the optimal path in real time under kinematic constraints, thereby improving the reliability of path planning.

[0158] The above description is merely a preferred embodiment of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the technical principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention. In the description of this specification, references to terms such as "an embodiment," "some embodiments," "example," "specific example," or "some examples" indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. Furthermore, the described specific features, structures, materials, or characteristics can be combined in any suitable manner in one or more embodiments or examples. In addition, those skilled in the art can combine and integrate different embodiments or examples and features of different embodiments or examples described in this specification without contradiction. Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" can explicitly or implicitly include at least one of that feature. In the description of this application, "a plurality of" means two or more, unless otherwise explicitly specified.

Claims

1. A hybrid local planning method based on dynamic state graphs, characterized in that, The application is applied to an autonomous mobile robot, comprising: calculating a target point displacement distance range and an orientation angle range based on real-time acquired autonomous mobile robot operation parameters and a pre-constructed initial dynamic graph; calculating a state node based on the target point displacement distance range, the orientation angle range and the initial dynamic graph, and constructing a dynamic state graph through the state node; constructing a target path based on the dynamic state graph and a preset search algorithm to obtain a target point, and obtaining a target linear velocity and a target point orientation according to the target point and a preset velocity strategy; obtaining a target angular velocity based on the target point and a preset path planning algorithm; sending the target linear velocity, the target angular velocity and the target point orientation to the autonomous mobile robot to make the autonomous mobile robot plan a local path.

2. The hybrid local planning method based on dynamic state graphs of claim 1, wherein, The application is applied to an autonomous mobile robot, comprising: calculating a target point displacement distance range and an orientation angle range based on real-time acquired autonomous mobile robot operation parameters and a pre-constructed initial dynamic graph; acquiring autonomous mobile robot operation parameters in real time to obtain a current velocity, a current maximum acceleration, a current angular velocity and a current maximum angular acceleration of the autonomous mobile robot; constructing an initial dynamic graph, wherein the initial dynamic graph comprises a plurality of decision layers and a plurality of initial nodes; calculating a braking distance value and a predicted distance value based on the current velocity and the current maximum acceleration, and obtaining a target point displacement distance range and a decision layer interval according to the braking distance value and the predicted distance value; 3. A hybrid local planning method based on dynamic state graphs as recited in claim 2, wherein, calculating an autonomous mobile robot actual angle change range based on the current angular velocity and the current maximum angular acceleration, and obtaining an orientation angle range and an adjacent initial node angle difference according to the autonomous mobile robot actual angle change range. The application is applied to an autonomous mobile robot, comprising: calculating a braking time according to a first time required for the current velocity to decelerate to a preset first velocity threshold value and a preset control period through the current velocity and the current maximum acceleration; calculating a braking distance value through the current velocity, the current maximum acceleration and the braking time; calculating a second time required for the current velocity to accelerate to a preset second velocity threshold value through the current velocity and the current maximum acceleration; calculating a predicted distance value through the current velocity, the current maximum acceleration, a preset prediction time, the second time and the preset control period; 4. The hybrid local planning method based on dynamic state graphs of claim 3, wherein, calculating a decision layer interval through the braking distance value, the predicted distance value and the number of decision layers. The application is applied to an autonomous mobile robot, comprising: calculating a left and right turning angle range of each decision layer according to the current angular velocity and the current maximum angular acceleration, and calculating an autonomous mobile robot actual angle change range according to the left and right turning angle range of each decision layer; calculating an autonomous mobile robot actual angle change range according to the left and right turning angle range of each decision layer; calculating a heading angle range according to the angle change range of each decision layer and the actual angle change range of the autonomous mobile robot; calculating an adjacent initial node angle difference according to the heading angle range and the initial node number.

5. A hybrid local planning method based on dynamic state graphs according to any one of claims 2 to 4, characterized in that, calculating a state node based on the target point displacement distance range, the heading angle range and the initial dynamic graph, and constructing a dynamic state graph through the state node, including: calculating state information of the initial node in each decision layer of the initial dynamic graph through the target point displacement distance range and the heading angle range, to obtain a state node, wherein the state information includes node position and node heading angle; converting the initial dynamic graph into a tree structure graph according to the state node; setting the same index value for all state nodes in the same decision layer of the tree structure graph; taking the current position of the autonomous mobile robot as a starting point, if the index value of the current decision layer is less than or equal to a preset threshold, calculating one edge connecting each state node of the current decision layer and adjacent state nodes of the previous decision layer; if the index value of the current decision layer is greater than the preset threshold, calculating n edges connecting each state node of the current decision layer and adjacent n state nodes of the previous decision layer, wherein if the state node is at both ends of the decision layer, calculating n-1 edges connecting the state node and adjacent n-1 state nodes of the previous decision layer; traversing the index value, if all decision layers corresponding to each index value have completed edge connection, obtaining a dynamic state graph.

6. A hybrid local planning method based on dynamic state graphs as recited in claim 5, wherein, constructing a target path based on the dynamic state graph and a preset search algorithm to obtain a target point, and obtaining a target linear velocity and a target point heading according to the target point and a preset speed strategy, including: calculating edge cost of each edge in the dynamic state graph through a preset node cost function; selecting a target path composed of a plurality of edges with edge cost meeting preset path requirements through a priority queue based on the preset search algorithm, and obtaining a target point according to the target path; calculating a first distance from the current position of the autonomous mobile robot to the target point and calculating a second distance of the autonomous mobile robot traveling in a preset target point decision time window; if the first distance is less than or equal to the second distance, taking the first distance as an allowable distance, calculating a first allowable linear velocity through the quotient of the first distance and the preset target point decision time window, and taking the first allowable linear velocity as the target linear velocity and the heading angle of the target point as the target point heading; if the first distance is greater than the second distance, calculating an actual allowable linear velocity according to a preset speed strategy, taking the actual allowable linear velocity as the target linear velocity, and performing angle discretization on the heading angle of the target point to generate a candidate heading angle set and obtaining a heading angle meeting preset alignment cost requirements from the candidate heading angle set as the target point heading through a preset angle screening principle.

7. A hybrid local planning method based on dynamic state graphs as recited in claim 6, wherein, If the first distance is greater than the second distance, an actual allowed linear velocity is calculated according to a preset speed strategy, the actual allowed linear velocity is taken as a target linear velocity, and a candidate orientation angle set is generated by angle discretization of the orientation angle of the target point, and an orientation angle meeting a preset alignment cost requirement is obtained from the candidate orientation angle set as a target point orientation according to a preset angle screening principle, including: If the first distance is greater than the second distance, a second allowed linear velocity is calculated according to an obstacle cost meeting a preset obstacle cost requirement in the target path; A third time from the current maximum acceleration from the current speed to the second allowed speed is calculated, and an actual allowed linear velocity is obtained according to the third time, the current speed, the current maximum acceleration and the second allowed linear velocity, and the actual allowed linear velocity is taken as a target linear velocity; The orientation angle of the target point is discretized according to a preset local search interval to obtain a candidate orientation angle set; The alignment cost value of each candidate orientation angle is calculated according to the preset angle screening principle, and an orientation angle meeting a preset alignment cost requirement is selected as a target point orientation.

8. A hybrid local planning method based on dynamic state graphs as recited in claim 7, wherein, Based on the target point and a preset path planning algorithm, a target angular velocity is obtained, including: According to the target point, a path search state diagram is constructed by the preset path planning algorithm, and a planning path is obtained from the path search state diagram within a preset simulation period; The target angular velocity is calculated according to the orientation angle of the current planning node, the simulation orientation angle of the current node, the target linear velocity and the target distance value, wherein if the first distance is less than or equal to the second distance, the target distance value is the first distance, and if the first distance is greater than the second distance, the target distance value is the product of the actual allowed linear velocity and the preset target point decision time window.

9. A hybrid local planning method based on dynamic state graphs as recited in claim 8, wherein, According to the target point, a path search state diagram is constructed by the preset path planning algorithm, and a planning path is obtained from the path search state diagram within a preset simulation period, including: According to the target point, a boundary of an extended area is constructed and a spatial resolution is discretized; According to a preset prediction time window, a planning node is expanded by the preset path planning algorithm to construct a path search state diagram; According to the path search state diagram, the cost value of each planning node within a preset simulation period is calculated, and a planning node meeting a preset path requirement is selected by a priority queue to obtain a planning path.

10. A hybrid local planning system based on dynamic state graphs, characterized in that, A hybrid local planning method based on a dynamic state diagram is executed, including: A target point constraint range calculation module, a dynamic state diagram construction module, a target linear velocity and target point orientation calculation module, a target angular velocity calculation module and a local path planning module; The target point constraint range calculation module is used to calculate a target point displacement distance range and an orientation angle range based on real-time acquired autonomous mobile robot running parameters and a pre-constructed initial dynamic diagram; The dynamic state diagram construction module is configured to calculate state nodes based on the target point displacement distance range, the orientation angle range and the initial dynamic diagram, and construct a dynamic state diagram through the state nodes; The target line speed and target point orientation calculation module is configured to construct a target path based on the dynamic state diagram and a preset search algorithm to obtain a target point, and obtain a target line speed and a target point orientation according to the target point and a preset speed strategy; The target angular velocity calculation module is configured to obtain a target angular velocity based on the target point and a preset path planning algorithm; The local path planning module is configured to send the target line speed, the target angular velocity and the target point orientation to the autonomous mobile robot, so that the autonomous mobile robot plans a local path.

Images