Two-stage safety control method and storage medium for autonomous mobile robot

Abstract

The application discloses a two-stage safety control method and storage medium of an autonomous mobile robot, relates to the technical field of path planning of a robot, and comprises the following steps: performing global path planning by using a DP algorithm based on a grid map, and generating a plurality of initial sub-target nodes arranged in sequence; performing global optimization on the initial sub-target nodes, and generating a global sub-target sequence; controlling the robot to take a current node in the global sub-target sequence as a tracking target, generating a control input based on a CBF-QP controller, and enabling the robot to move to the current node under the condition that a safe distance is kept from an obstacle; when it is detected that the robot is in a deadlock state, suspending tracking of the current node, generating a local sub-target sequence based on an A algorithm, and controlling the robot to move along the local sub-target sequence; and when the robot meets a recovery condition, resuming movement along a node in the global sub-target sequence that has not been reached. The application can overcome the problem of robot deadlock, improve the calculation efficiency, and reduce energy consumption.

Description

Technical Field

[0001] This application relates to the field of robot path planning technology, and in particular to a two-stage safety control method and storage medium for an autonomous mobile robot. Background Technology

[0002] Achieving safe and efficient navigation for autonomous mobile robots in complex environments containing both static and dynamic obstacles is a key technical challenge in the field of robot control. To ensure that robots do not collide with obstacles during task execution, in recent years, an optimization control framework based on Control Barrier Functions (CBF) combined with Quadratic Programming (QP) has been widely applied to the low-level real-time safety control of robots. This framework, by encoding safety constraints into CBF inequalities and incorporating them into QP solving, mathematically guarantees that the system state always remains within a preset safety set. Simultaneously, to improve the robot's travel efficiency in large-scale environments, existing technologies have also attempted to introduce global path planning methods such as Dynamic Programming (DP), which provide a macroscopic travel route for the robot by searching for the globally optimal path on a grid map.

[0003] However, the aforementioned existing technologies still have significant drawbacks. On the one hand, the traditional CBF-QP method is essentially a greedy strategy based on local gradient information. When the robot faces complex geometric configurations such as "U-shaped traps" or dense obstacle regions, the attraction of the target point and the repulsion of the obstacle boundary can easily form a local force balance, causing the robot to stagnate, i.e., be in a "deadlock" state. At the same time, this method lacks prior knowledge of the global environmental topology, and the robot often only makes turning or turning-back actions when approaching obstacles, significantly increasing the travel distance and energy consumption. On the other hand, although the DP algorithm can provide a globally optimal path, its computational complexity increases sharply with the increase of grid resolution. The computation time can reach tens of seconds when finely dividing the grid, which cannot meet the needs of real-time response to dynamic obstacles. On the other hand, coarse dividing the grid may lead to suboptimal paths or failure to identify narrow passages. Even if the DP algorithm is simply combined with CBF-QP, due to the mismatch in time scale and the lack of coordination mechanism between the two, the robot still faces problems such as excessive computational burden and insufficient ability to cope with dynamic obstacles.

[0004] Therefore, how to effectively overcome the robot deadlock problem, improve computing efficiency and reduce energy consumption are urgent technical problems that need to be solved. Summary of the Invention

[0005] In view of this, embodiments of this application provide a two-stage safety control method and storage medium for autonomous mobile robots, which can effectively overcome the robot deadlock problem, improve computing efficiency and reduce energy consumption.

[0006] Firstly, this application provides a two-stage safety control method for autonomous mobile robots, including: Based on the grid map, the DP algorithm is used for global path planning to generate an initial global path. The grid map is used to represent the spatial layout information of the mobile robot's working environment. The initial global path is globally optimized, redundant nodes are removed, and a global sub-target sequence is generated. The global sub-target sequence includes multiple global sub-target nodes arranged in order. The mobile robot is controlled to move along the global sub-target sequence, and the current global sub-target node in the global sub-target sequence is used as the tracking target. The quadratic programming controller based on the control obstacle function constraint generates the control input to drive the mobile robot, so that the mobile robot moves towards the current global sub-target node while maintaining a preset safe distance from the obstacle. The control obstacle function is used to constrain the distance between the mobile robot and the obstacle to be no less than the preset safe distance. When a deadlock is detected in the mobile robot, it stops moving towards the current global sub-target node and, based on A... The algorithm uses the current position of the mobile robot, the current global sub-target node, or any global sub-target node after the current global sub-target node to generate a local sub-target sequence, and controls the mobile robot to move along the local sub-target sequence in order to escape the deadlock region. When the mobile robot meets the preset recovery conditions, control the mobile robot to move along the global sub-target nodes that have not been reached in the global sub-target sequence.

[0007] Secondly, this application provides a storage medium on which a computer program is stored, which, when executed by a processor, implements the above-mentioned two-stage safety control method for autonomous mobile robots.

[0008] By utilizing the above technical solution, the present application provides a two-stage safety control method and storage medium for an autonomous mobile robot. First, a global path is generated on a grid map using a DP algorithm, and then a simplified global sub-target sequence is obtained through redundancy removal optimization. This provides macroscopic direction guidance for the mobile robot, enabling it to move along the globally optimal route in a wide-ranging environment. This avoids the deadlock problem caused by force balance in complex obstacle areas where only local gradient information from a quadratic planning controller is available. Simultaneously, it effectively shortens the travel distance and reduces energy consumption. Furthermore, when the mobile robot is in a deadlock state, tracking of the current global sub-target node is paused and the process switches to A. The algorithm performs local sub-objective sequence planning to quickly generate obstacle avoidance paths to guide the mobile robot out of the deadlock region. Due to A... The algorithm's local planning is initiated only on demand when deadlock is triggered, thus avoiding the time burden of continuous computation at high resolutions inherent in dynamic programming (DP) algorithms. Furthermore, once local planning is complete, tracking of unreached target nodes in the original global sub-target sequence resumes, ensuring both the topological invariance of the global path and real-time response to sudden dynamic obstacles. Additionally, the on-demand activation of deadlock detection and local planning allows the quadratic planning controller, based on control obstacle function constraints, to continuously output control commands that satisfy safety constraints with low computational cost for the vast majority of the time, intervening only in local replanning when necessary. This reduces the overall average computational load of the mobile robot system. Therefore, this application effectively overcomes the robot deadlock problem, reduces computational and travel energy consumption, and improves the operational efficiency and robustness of mobile robots in complex dynamic environments.

[0009] The above description is only an overview of the technical solution of this application. In order to better understand the technical means of this application and to implement it in accordance with the contents of the specification, and to make the above and other objects, features and advantages of this application more obvious and understandable, the following are specific embodiments of this application. Attached Figure Description

[0010] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings: Figure 1 A flowchart illustrating a two-stage safety control method for an autonomous mobile robot provided in an embodiment of this application is shown. Figure 2 A flowchart illustrating another two-stage safety control method for autonomous mobile robots provided in an embodiment of this application is shown. Figure 3 This illustration shows an approximate processing effect diagram of an irregular obstacle provided by an embodiment of this application; Figure 4 A schematic diagram showing the optimization result of Algorithm 1 provided in an embodiment of this application is illustrated; Figure 5 An embodiment of this application provides an A A comparison of the computation time of the algorithm and the DP algorithm for path planning under the same grid width; Figure 6 This illustration shows a motion trajectory diagram of a mobile robot in an environment with both static and dynamic obstacles, provided by an embodiment of this application, using the method described herein. Figure 7A schematic diagram of the system structure of a computer device provided in an embodiment of this application is shown. Detailed Implementation

[0011] The present application will be described in detail below with reference to the accompanying drawings and embodiments. It should be noted that, unless otherwise specified, the embodiments and features described in the embodiments of the present application can be combined with each other.

[0012] This invention provides a two-stage safety control method for autonomous mobile robots, such as... Figure 1 As shown, the method includes: S101, based on a grid map, uses the Dynamic Programming (DP) algorithm to perform global path planning and generate an initial global path. The grid map is used to represent the spatial layout information of the mobile robot's working environment.

[0013] A grid map discretizes the two-dimensional workspace of a mobile robot into several equally sized grid cells. Each grid cell is labeled as either a feasible region or an obstacle region, representing the spatial layout information of the mobile robot's working environment. A feasible region is a grid cell without obstacles that the mobile robot can pass through; an obstacle region is a grid cell with obstacles that prevent the mobile robot from passing through.

[0014] The initial global path is the optimal travel path from the preset starting position to the preset target position found in the feasible region. This path consists of a series of path points arranged in the order of travel, where each path point is defined as an initial sub-target node, and all initial sub-target nodes form an initial sub-target sequence in the order of travel.

[0015] Specifically, before the mobile robot starts, a grid map is first established to represent the spatial layout information of its working environment. Then, based on the grid map, the DP algorithm is used to perform global path planning to search for the optimal travel path from the preset starting position to the preset target position in the feasible area, thereby obtaining the initial global path.

[0016] S102, perform global optimization on the initial global path, remove redundant nodes, and generate a global sub-target sequence, which includes multiple global sub-target nodes arranged in order.

[0017] Specifically, after obtaining the initial global path, since it is a sequence of path points generated based on a grid map, the dynamic programming (DP) algorithm outputs a large number of densely arranged path points during the search process to ensure the continuity and traversability of the path. Many of these path points are intermediate nodes located along the same straight line or whose overall direction remains essentially unchanged. If the mobile robot tracks these densely arranged nodes one by one, it will lead to frequent turning and acceleration / deceleration operations, increasing energy consumption and control burden. Therefore, to reduce the number of target points that the mobile robot needs to track during subsequent movement and to reduce the frequency of turning and acceleration / deceleration, redundant nodes in the initial global path must be removed while maintaining the overall path direction and obstacle avoidance capability. This involves processing the initial sub-target sequence in the initial global path, removing redundant nodes, and simplifying the initial sub-target sequence into a sequence with fewer nodes. The simplified sequence is the global sub-target sequence.

[0018] S103, control the mobile robot to move along the global sub-target sequence, and take the current global sub-target node in the global sub-target sequence as the tracking target. The quadratic programming controller based on the control obstacle function constraint generates the control input to drive the mobile robot, so that the mobile robot moves towards the current global sub-target node while maintaining a preset safe distance from the obstacle.

[0019] A quadratic programming controller is a controller that finds the optimal solution that makes the actual control input as close as possible to the desired control input, while satisfying the safety constraints imposed by the control obstacle function.

[0020] The obstacle control function is used to constrain the distance between the mobile robot and the obstacle to be no less than a preset safe distance.

[0021] The safety distance is a preset fixed value. This value can be set according to the physical size of the mobile robot, or according to the upper limit of the mobile robot's movement speed and braking performance (i.e., the sum of the robot's braking distance at maximum speed and the preset safety margin), or according to the safety level requirements of the task scenario. This application embodiment does not limit this.

[0022] Specifically, after the global sub-target sequence is generated, the mobile robot can be controlled to move along the global sub-target sequence. The current global sub-target node in the global sub-target sequence is used as the tracking target. The quadratic programming controller based on the control obstacle function constraint generates the control input to drive the mobile robot, so that the mobile robot moves towards the current global sub-target node while maintaining a preset safe distance from the obstacle.

[0023] S104, when the mobile robot is detected to be in a deadlock state, it stops moving towards the current global sub-target node and, based on the A-Star Algorithm (A... The algorithm uses the current position of the mobile robot, the current global sub-target node, or any global sub-target node after the current global sub-target node to generate a local sub-target sequence, and controls the mobile robot to move along the local sub-target sequence in order to escape the deadlock region.

[0024] Deadlock is a state in which a mobile robot is unable to effectively approach the current global sub-target node within a preset time period and its movement speed remains close to zero.

[0025] Specifically, to avoid the mobile robot from stalling (i.e., being unable to continue executing the planned path for an extended period), during the process of controlling the mobile robot to move along the global sub-target sequence, it is possible to detect in real time whether the mobile robot is in a deadlock state. If a deadlock is detected, the robot will stop moving towards the current global sub-target node, and based on A... The algorithm uses the current position of the mobile robot, the current global sub-target node, or any global sub-target node after the current global sub-target node to generate a local sub-target sequence, and controls the mobile robot to move along the local sub-target sequence in order to escape the deadlock region.

[0026] S105, when the mobile robot meets the preset recovery conditions, control the mobile robot to move along the global sub-target nodes that have not been reached in the global sub-target sequence.

[0027] The recovery condition is a pre-set trigger condition used to determine that the mobile robot has successfully left the deadlock area and can resume its movement along the global sub-target sequence.

[0028] In one embodiment, the recovery condition includes the distance between the current position of the mobile robot and the end node of the local sub-target sequence being less than or equal to a preset distance.

[0029] The endpoint of the local sub-target sequence is A. The final node of the local obstacle avoidance path planned by the algorithm. This node is the final transition point in the process of the mobile robot leaving the deadlock area. When the mobile robot reaches the vicinity of this endpoint node, it indicates that the mobile robot has successfully bypassed the obstacle area that caused the deadlock, and the current position has been restored to a safe and passable area. The endpoint node is usually configured to be located near a global sub-target node that has not been reached in the global sub-target sequence or on a collision-free path leading to that global sub-target node. Therefore, resuming the tracking of the global sub-target sequence at this time has a safety guarantee.

[0030] The preset distance is a pre-defined distance threshold, which can be set according to the positioning accuracy of the mobile robot, the resolution of the grid map, and the size of the mobile robot itself.

[0031] Specifically, to ensure that after the mobile robot leaves the deadlocked area along the local sub-target sequence, it can promptly switch back to the global sub-target sequence to continue executing the remaining travel tasks, and to avoid the mobile robot deviating from the globally optimal path for a long time or continuously executing local planning, leading to non-optimal paths and wasted energy, it is necessary to determine whether the mobile robot meets the recovery conditions after controlling its movement along the local sub-target sequence. When the mobile robot meets the recovery conditions, it should immediately pause tracking the current local sub-target node in the local sub-target sequence and resume tracking the global sub-target nodes that have not yet been reached in the global sub-target sequence, controlling the mobile robot to continue moving along the unreached global sub-target nodes. After resuming movement along the global sub-target sequence, the quadratic programming controller controlling the obstacle function constraints remains active, ensuring that the mobile robot always maintains a preset safe distance from obstacles during subsequent travel.

[0032] As can be seen from the above, the two-stage safety control method for autonomous mobile robots provided in this application first uses the DP algorithm to generate a global path on a grid map and then optimizes it by redundancy removal to obtain a simplified global sub-target sequence, providing macroscopic direction guidance for the mobile robot. This enables the mobile robot to move along the globally optimal route in a large-scale environment, thereby avoiding the problem of deadlock caused by force balance in complex obstacle areas when relying solely on the local gradient information of the quadratic planning controller. At the same time, it effectively shortens the travel distance and reduces energy consumption. On this basis, when the mobile robot is in a deadlock state, it pauses the tracking of the current global sub-target node and switches to A. The algorithm performs local sub-objective sequence planning to quickly generate obstacle avoidance paths to guide the mobile robot out of the deadlock region. Due to A... The algorithm's local planning is initiated only on demand when deadlock is triggered, thus avoiding the time burden of continuous computation at high resolutions inherent in dynamic programming (DP) algorithms. Furthermore, once local planning is complete, tracking of unreached target nodes in the original global sub-target sequence resumes, ensuring both the topological invariance of the global path and real-time response to sudden dynamic obstacles. Additionally, the on-demand activation of deadlock detection and local planning allows the quadratic planning controller, based on control obstacle function constraints, to continuously output control commands that satisfy safety constraints with low computational cost for the vast majority of the time, intervening only in local replanning when necessary. This reduces the overall average computational load of the mobile robot system. Therefore, this application effectively overcomes the robot deadlock problem, reduces computational and travel energy consumption, and improves the operational efficiency and robustness of mobile robots in complex dynamic environments.

[0033] Furthermore, as a refinement and extension of the specific implementation methods of the above embodiments, and to fully illustrate the specific implementation process of this embodiment, another two-stage safety control method for autonomous mobile robots is provided, such as... Figure 2 As shown, the method includes: S201: Based on the grid map, determine the starting node, target node, and obstacle area of ​​the mobile robot; based on the starting node, target node, and obstacle area, use the DP algorithm to perform global path planning and obtain the initial global path.

[0034] Considering that obstacles in the working environment of a mobile robot may have irregular geometries, in one embodiment, before using the dynamic programming (DP) algorithm for global path planning, the environment needs to be preprocessed to approximate each irregular obstacle as a single or multiple circles. After approximation, each obstacle is characterized by its center position and radius. Then, the approximate circular obstacles are mapped to corresponding grid cells in a grid map and labeled as obstacle regions to reduce spatial complexity and computational cost. For example, see [link to relevant documentation]. Figure 3 , Figure 3 The obstacle in the image is an irregular obstacle 301. This irregular obstacle 301 is approximated as a circle 302. This circle 302, when mapped onto the corresponding grid cell in the grid map, forms the obstacle region 303. Figure 3 The irregular blue area.

[0035] After the above preprocessing, the state space in the raster map is defined as the set of all non-obstacle rasters. ,Right now .

[0036] in, and These are the width and height of the raster map, respectively. and These represent the horizontal and vertical coordinates of the mobile robot, respectively.

[0037] In this state, the starting position of the mobile robot is The target location is The grid cells occupied by all obstacles constitute the obstacle state set. Thus, the global path planning problem is formalized as finding the path from the starting node in the state space. Start, avoid the obstacle state set Find a path to the target node The optimal path.

[0038] Specifically, after the grid map is constructed, a starting node is marked on the grid map according to the actual starting position of the mobile robot, and a target node is marked on the grid map according to the target position of the task that the mobile robot needs to perform. All grid cells marked as obstacle areas are then identified as obstacle regions. After determining the starting node, target node, and obstacle regions, the starting node is used as the starting point for path search, and the target node is used as the ending point. Grid cells occupied by obstacle regions are set as impassable constraints. A dynamic programming (DP) algorithm is then used to perform a global optimal path search within the feasible regions of the grid map to obtain the initial global path.

[0039] In one embodiment, based on the starting node, the target node, and the obstacle region, a dynamic programming (DP) algorithm is used to perform global path planning to obtain an initial global path. This includes: mapping the obstacle region to an obstacle grid on the grid map, with the starting node as the starting point and the target node as the ending point; defining the state space, action space, and state transition function of the mobile robot in the grid map; setting the immediate reward value corresponding to each action; performing value iteration based on the state transition function and the immediate reward value to obtain the optimal value of each node in the grid map; and backtracking based on the optimal value of each node to obtain the optimal node sequence from the starting node to the target node, which serves as the initial global path.

[0040] The state space is the set of all non-obstacle grid cells.

[0041] A state transition function describes the position change rules of a mobile robot after performing any action. Specifically, when the mobile robot performs an action in the current grid... Afterwards, if the target location If a state belongs to the state space but not to the obstacle state set, then move the mobile robot from the current grid to... Otherwise, keep the current grid unchanged.

[0042] The action space is the set of all actions that a mobile robot can perform at each step in a grid map. In this embodiment, the action space includes four movement actions: up, down, left, and right. Each action corresponds to a fixed grid coordinate offset, where up is (0, 1), down is (0, -1), left is (-1, 0), and right is (1, 0). Each time the mobile robot performs an action, it moves from the current grid cell to the adjacent grid cell.

[0043] Specifically, after the grid map is constructed, the starting node is marked on the grid map according to the actual deployment location of the mobile robot, the target node is marked on the grid map according to the target location of the task that the mobile robot needs to perform, and all grid cells marked as obstacle areas in the grid map are identified as obstacle areas.

[0044] Furthermore, after determining the starting node, target node, and obstacle region, the obstacle region is mapped onto the grid map as obstacle grids, with the starting node as the starting point and the target node as the ending point. The state space, action space, and state transition function of the mobile robot in the grid map are defined. When the target position is within the state space (i.e., the target grid is a non-obstacle grid), the mobile robot moves from the current grid to the grid corresponding to the target position. When the target position is outside the state space (i.e., falling into an obstacle grid or exceeding the map boundary), the current grid position remains unchanged. Then, an immediate reward value is set for each action: a positive reward (e.g., +10) is given upon reaching the target node, a negative reward (e.g., -10) is given upon colliding with an obstacle grid, and a slight negative reward (e.g., -1) is given for normal movement. Value iteration is performed based on the state transition function and the immediate reward value.

[0045] During the value iteration process, the optimal value of each node in the grid map is obtained by repeatedly updating the value function of each grid cell until convergence.

[0046] The value function includes a state value function and an action value function. The state value function represents the expected cumulative reward that the mobile robot can obtain in a certain grid, and its iterative update method is as follows: ; in, Indicates the optimal strategy Under, state The value of mobile robots from the grid Depart according to the optimal strategy The expected cumulative reward that can be obtained from moving. Indicates the current time The state, that is, the current grid position of the mobile robot. This means taking the maximum value among all possible actions, i.e., selecting the action that maximizes the value. This means calculating the expectation under strategy π. Indicates the execution of an action The immediate reward received afterward This represents a discount factor, used to adjust the relative importance of future rewards compared to current rewards. This represents the cumulative reward that can be obtained starting from the next grid cell. Indicates the current time The state, that is, the current grid position of the mobile robot. Indicates the current time The action performed is the movement direction (up / down / left / right) chosen by the mobile robot. Indicates the next moment The state, i.e., the state of performing an action. The new grid position that is reached later, Indicates the next moment The action performed.

[0047] The action value function represents the expected cumulative reward that a mobile robot can obtain after performing a specific action in a grid. Its iterative update method is as follows: ; in, Indicates the optimal strategy Below, from the state Start executing the action The action value, that is, the action performed by the mobile robot. The expected cumulative reward that can be obtained later Indicates the next state Next action The value of the action, Indicates the next state The maximum value among all possible actions indicates that the optimal action will be selected in the future.

[0048] By iteratively updating the value of each grid cell using the above formula until convergence, the optimal value from each feasible grid cell to the target node in the grid map can be calculated. Finally, based on the optimal value of each node, a forward backtracking process is performed starting from the initial node according to the optimal value gradient. The adjacent grid cells that maximize the cumulative value are selected as the next nodes on the path, gradually extending to the target node. Ultimately, an optimal node sequence consisting of a series of path nodes arranged in the forward sequence is obtained. ,in, As the starting node, For the target node, this sequence is the initial global path.

[0049] S202: Extract the path points traversed by the initial global path and form an initial sub-target sequence in the order of movement. The initial sub-target sequence includes multiple initial sub-target nodes arranged in sequence. Taking the current initial sub-target node in the initial sub-target sequence as the first endpoint and the initial sub-target node that is farthest from the current initial sub-target node as the second endpoint, determine whether the line connecting the first endpoint and the second endpoint crosses the obstacle area. If it does, update the second endpoint to the node whose index value is between the index of the first endpoint and the index of the current second endpoint, and return to the step of determining whether the line connecting the first endpoint and the second endpoint crosses the obstacle area. If it does not cross, determine all intermediate nodes between the first endpoint and the second endpoint as redundant nodes and delete them from the initial sub-target sequence, update the second endpoint to the new current initial sub-target node, and return to the step of taking the current initial sub-target node in the initial sub-target sequence as the first endpoint, until the current initial sub-target node is the last node in the initial sub-target sequence. Output the remaining nodes in the original order to obtain the global sub-target sequence.

[0050] Specifically, after obtaining the initial global path, all path points traversed by the initial global path are extracted and arranged sequentially from the starting point to the end point to form an initial sub-target sequence. Each point in this sequence is located within the feasible area of ​​the raster map.

[0051] Then, a binary jump search strategy is employed to quickly identify and remove unnecessary intermediate nodes, i.e., redundant nodes. Specifically, starting from the first node of the initial sub-target sequence, this node is designated as the current initial sub-target node, serving as the first endpoint; simultaneously, the node in the sequence that is furthest from the current initial sub-target node (i.e., the last node of the sequence) is designated as the second endpoint. Subsequently, it is determined whether the line connecting the first and second endpoints crosses an obstacle region. This determination is achieved by comparing all grid cells traversed by the line with the obstacle region. If any grid cell on the line belongs to the obstacle region, the line is considered to have crossed the obstacle region; otherwise, it is considered safe.

[0052] In the first case: if the line passes through the obstacle area, it means that the first endpoint and the current second endpoint cannot be directly connected due to the obstacle. In this case, the second endpoint is moved forward. Specifically, the second endpoint is updated to the node whose index value is located between the index of the first endpoint and the index of the current second endpoint. That is, the search span is reduced by using the binary search method. Then, the process is repeated to check if the line passes through the obstacle area. This process is repeated until a second endpoint is found whose line does not pass through the obstacle area.

[0053] The second scenario: If the line does not pass through the obstacle area, it means that there is a straight path without obstacles between the first endpoint and the current second endpoint. The mobile robot can travel directly from the first endpoint to the second endpoint without passing through any intermediate nodes between them. In this case, all intermediate nodes between the first endpoint and the second endpoint are determined to be redundant nodes and removed from the initial sub-target sequence. Then, the second endpoint is updated to the new current initial sub-target node. That is, after completing the jump of the current node, the subsequent node optimization is continued with this node as the new starting point, and the process is returned to re-execute the step with the current initial sub-target node as the first endpoint.

[0054] It should be noted that when the line connecting the first and second endpoints crosses the obstacle area and their indices are adjacent, the search span cannot be reduced using the binary search method because there is no intermediate node between them. In this case, the first endpoint can be retained, its next adjacent node can be updated as the new current initial sub-target node, and this new current initial sub-target node can be used as the new first endpoint to continue the optimization process for subsequent nodes. When the line connecting the first and second endpoints does not cross the obstacle area and their indices are adjacent, there is no intermediate node between them, so no deletion operation is needed. In this case, the second endpoint can be updated as the new current initial sub-target node, and the optimization process for subsequent nodes can continue. The above-mentioned handling of adjacent nodes ensures that the binary jump search strategy can still advance the traversal normally when it is impossible to further reduce the search span, thereby ensuring that redundant nodes can be completely removed for initial sub-target sequences of any length.

[0055] Furthermore, the aforementioned redundant node removal process can be implemented using the pseudocode shown in Algorithm 1. The input to Algorithm 1 is the initial sub-target sequence. The output is the optimized initial sub-target sequence, i.e., the global sub-target sequence. ,in, The core idea of ​​Algorithm 1 is as follows: Starting from the initial node, each attempt is made to connect the current node to the farthest node in the sequence. If the connection is safe, the node is skipped and all intermediate nodes are deleted. If the connection is unsafe, a binary search is used to gradually narrow the search range until the farthest safe reachable node is found. The safety criterion is as follows: The definition is: for a pair of child target nodes , If line segment If the state set of obstacles is not crossed, then ,otherwise .

[0056] For example, the pseudocode for Algorithm 1 can be seen in Table 1 below.

[0057] Table 1. Pseudocode for Algorithm 1

[0058] To visually demonstrate the optimization effect of Algorithm 1, Figure 4 The diagram shows an optimization result of Algorithm 1 provided in an embodiment of this application. Figure 4 The experimental environment shown includes a known map containing 5 static obstacles. Figure 4 The grid width of the raster map is set to 0.15 meters. Specifically, the DP algorithm is first used for global path planning, and then Algorithm 1 is applied to remove redundant nodes from the initial sub-target sequence. The five static obstacles are all processed using the aforementioned circular approximation method.

[0059] After applying Algorithm 1 in the above environment, the optimization result is as follows: Figure 4 As shown, Figure 4 In the diagram, green dots represent the starting node, yellow dots represent the target node, each blue irregular area represents an obstacle area, red dashed lines represent the driving path before applying Algorithm 1 (i.e., the path before optimization), and yellow solid lines represent the driving path after applying Algorithm 1 (i.e., the path after optimization). Figure 4 It can be seen that a large number of densely packed redundant nodes in the initial global path were successfully removed, and only the key turning points were retained as global sub-target nodes. The optimized path significantly reduced the number of nodes while maintaining the overall direction and obstacle avoidance capability, thus verifying the effectiveness of Algorithm 1 in reducing the number of target points.

[0060] Using the above strategy, each time starting from the current node, priority is given to jumping to the farthest node at the end of the sequence. Only when encountering obstacles is the robot gradually halved forward to approach the target node, thus finding the farthest node that the current node can reach directly with the fewest number of judgments. All intermediate nodes between the farthest node and the current node are then eliminated at once. This process is repeated, and after each jump, the node reached is used as the new current initial sub-target node to continue moving forward until the current initial sub-target node reaches the last node in the initial sub-target sequence, indicating that all key turning points from the start to the end have been filtered. At this point, all nodes remaining in the initial sub-target sequence are key nodes that cannot be skipped in the path. These remaining nodes are output in their original order of movement, resulting in the global sub-target sequence. The number of nodes in this sequence is much smaller than in the initial sub-target sequence, thus significantly reducing the number of target points that the subsequent mobile robot needs to track while maintaining the overall path direction and obstacle avoidance capability. This effectively reduces the frequency of turning and acceleration / deceleration during the movement process.

[0061] S203, control the mobile robot to move along the global sub-target sequence, and take the current global sub-target node in the global sub-target sequence as the tracking target. The quadratic programming controller based on the control obstacle function constraint generates the control input to drive the mobile robot, so that the mobile robot moves towards the current global sub-target node while maintaining a preset safe distance from the obstacle.

[0062] In one embodiment, a quadratic programming controller based on control obstacle function constraints generates control inputs for driving a mobile robot, including: acquiring the current position information of the mobile robot; calculating the desired control input based on the positional deviation between the current position information and the current global sub-target node; acquiring the distance information between the mobile robot and obstacles, and constructing a control obstacle function based on the difference between the distance information and the safe distance; constructing a quadratic programming problem with the desired control input as the objective and the control obstacle function as the safety constraint; and solving the quadratic programming problem to obtain the actual control input that is closest to the desired control input under the premise of satisfying the safety constraint, which is then used as the control input for driving the mobile robot.

[0063] The distance information includes either the distance from the mobile robot to a static obstacle or the distance to a dynamic obstacle.

[0064] It should be noted that, to ensure the mobile robot maintains a safe distance from obstacles while tracking global sub-target nodes, a safety set must be pre-defined, and a control obstacle function must be constructed based on this safety set. The safety set is the collection of all safe states in the state space, where a safe state is defined as a state in which the distance between the mobile robot and all obstacles in the working environment is greater than or equal to the safe distance.

[0065] Specifically, the system state of the mobile robot is first defined, i.e., its current location information. ,in, and These represent the mobile robot in a two-dimensional plane. shaft and The position of the axis Let be the orientation angle of the mobile robot. The kinematic model of the mobile robot is described using the following affine control system: ; in, To control the input, It is a drifting state. To control the input matrix.

[0066] In the Robotarium platform, this kinematic model can be specifically expanded as follows: ; in, Indicates the first A mobile robot Velocity in the axial direction, Indicates the first A mobile robot Velocity in the axial direction, Indicates the first The angular velocity of a mobile robot Indicates the first The orientation angle of a mobile robot as it moves and The first The linear velocity and angular velocity of a mobile robot.

[0067] In this embodiment, the first [item] in the working environment The mobile machine and the first The control barrier function between obstacles can define its safety function. ,in, For the first Current location information of each obstacle. For the first The center position of each obstacle For the first The equivalent radius of an obstacle (i.e., the radius of an obstacle after it is approximated as a circle). This is a preset safe distance.

[0068] Understandably, this security function Equivalent to the first The mobile robot and the first The distance between the obstacles is greater than or equal to This means the mobile robot is outside the safe zone of the obstacle. When there are multiple obstacles in the working environment, the mobile robot needs to avoid all obstacles simultaneously, therefore the first... Safety collection of robots It can be defined as the intersection of the safety constraints between the robot and all obstacles and other mobile robots: ; in, Indicates the first The intersection of safety constraints between a mobile robot and all obstacles, i.e., for any obstacle, the first... The status of the mobile robot All meet This ensures that the robot does not collide with any obstacles; Indicates the first The intersection of the safety constraints between the i-th mobile robot and all other mobile robots, i.e., for any other mobile robot, the i-th... The status of the mobile robot All meet To ensure that multiple mobile robots do not collide with each other, Indicates the first The mobile machine and the first Control barrier functions between other mobile robots This indicates that traversing all numbers is not equal to The mobile robot, that is, excluding the mobile robot itself, only considering the safety constraints posed by other mobile robots in the environment to this mobile robot; This means taking the intersection of the two sets of security constraints mentioned above, i.e., the first set of constraints. The safety state of a mobile robot must simultaneously satisfy two conditions: maintaining a safe distance from all obstacles and maintaining a safe distance from all other robots. The intersection of these conditions constitutes the safety status of the first robot. A complete safety set for robots .

[0069] To ensure safe collection The forward invariance, that is, once the state of the mobile robot enters the safe set... Then, at all subsequent times, it will not leave this safe set, controlling the input. The control barrier function constraint must be satisfied. This constraint is achieved by using a safety function... The control barrier function is constructed and incorporated into the constraints of the quadratic programming problem to achieve this.

[0070] In one embodiment, the expression for the control barrier function is: ; ; in, For safety functions, This provides the current location information of the mobile robot. For safety functions drifting state Li Daoshu, For the safety function along the control input matrix Li Daoshu, It is a monotonically increasing function. This refers to the center position information of the obstacle. For the Euclidean norm, The equivalent radius of the obstacle. For safe distance.

[0071] After constructing the aforementioned safety function and control barrier function, these are used as safety constraints. With the desired control input as the optimization objective, a quadratic programming problem is constructed. The expression for the quadratic programming problem is: ; in, For actual control input, It is a positive definite weighted matrix. for 3D real space, To control the dimensions of the input (e.g.) ), To control the input, This is a transpose operation. In a quadratic programming problem, the objective function is set to minimize the weighted squared deviation between the actual control input and the desired control input, that is, to make the actual motion of the mobile robot as close as possible to the theoretical desired motion while satisfying all safety constraints.

[0072] In one embodiment, when constructing the quadratic programming problem, it is desirable to control the input. The calculation method is as follows: Within each control cycle, the positional deviation between the current position information of the mobile robot and the current position information of the global sub-target node is calculated. Then, the desired control input is calculated using a proportional control law. The formula for calculating proportional control is: ; in, This represents the current location of the tracked global sub-target node. For the first The current location information of each mobile robot. The proportional gain coefficient (e.g.) ), For the first The linear velocity of a mobile robot For the first The angular velocity of a mobile robot.

[0073] Specifically, during the mobile robot's movement along the global sub-target sequence, in each control cycle, the robot first acquires its current position information in the workspace using its onboard positioning sensors. Then, based on the current position information and the position information of the current global sub-target node, the positional deviation between the two is calculated. This deviation reflects the difference between the robot's current position and the desired target position. Based on this deviation, a proportional control law is used to calculate the desired control input. This desired control input represents the speed command required for the mobile robot to move towards the current global sub-target node, where the desired linear velocity is proportional to the magnitude of the positional deviation, and the desired angular velocity is proportional to the angular deviation between the robot's orientation and the target direction.

[0074] Simultaneously, the mobile robot's onboard distance sensors acquire real-time distance information relative to surrounding obstacles. Based on the difference between the acquired distance information and a preset safety distance, a safety function is constructed and used as the obstacle control function. After constructing the obstacle control function, it is used as a safety constraint, and the desired control input is used as the optimization objective to construct a quadratic programming problem. In the quadratic programming problem, the objective function is set to minimize the weighted squared deviation between the actual control input and the desired control input; that is, under the premise of satisfying all safety constraints, the robot's actual movement is made as close as possible to the theoretically desired movement. Finally, a quadratic programming solver is invoked to solve the quadratic programming problem, obtaining the actual control input that is closest to the desired control input while satisfying the safety constraints. This actual control input is then used as the control command to drive the mobile robot's actuator. The mobile robot's actuator drives itself to move towards the current global sub-target node according to this control command.

[0075] It should be noted that during the entire process of moving along the global sub-target sequence, the above control process is repeatedly executed with a fixed sampling period. The quadratic programming controller that controls the obstacle function constraint continuously limits the state of the mobile robot within the safe area in each period, so that the mobile robot can always maintain a preset safe distance from static obstacles, dynamic obstacles and other mobile robots while moving towards the current global sub-target node, thereby achieving safe and stable tracking control.

[0076] S204, when the mobile robot is detected to be in a deadlock state, it stops moving towards the current global sub-target node and, based on A... The algorithm uses the current position of the mobile robot, the current global sub-target node, or any global sub-target node after the current global sub-target node to generate a local sub-target sequence, and controls the mobile robot to move along the local sub-target sequence in order to escape the deadlock region.

[0077] In one embodiment, the detection of deadlock includes: obtaining the target distance between the current position information of the mobile robot and the current global sub-target node, and the current speed information of the mobile robot; when the target distance is continuously greater than a preset convergence radius within a preset time period, and the current speed information is continuously less than or equal to a speed threshold within a preset time period, the mobile robot is determined to be in a deadlock state.

[0078] The convergence radius is a pre-set distance threshold used to determine whether the mobile robot has successfully reached the vicinity of the current global sub-target node or local sub-target node. The value of the convergence radius can be set according to the resolution of the grid map and the positioning accuracy of the mobile robot; for example, the convergence radius can be set to 0.1m.

[0079] The speed threshold is a preset speed limit value used to determine whether the mobile robot is in a state of motion stagnation. This value can be set according to the control accuracy of the mobile robot's actuator, the measurement noise of the speed sensor, and the minimum effective speed of motion control. For example, the speed threshold can be set to 0.1 m / s.

[0080] Specifically, during the mobile robot's movement along the global sub-target sequence, the current position information of the mobile robot is acquired through the positioning sensor in each control cycle. The Euclidean distance between the current position and the current global sub-target node is calculated and recorded as the target distance, reflecting how far the mobile robot is from the currently tracked target. Simultaneously, the current speed information of the mobile robot is acquired through the speed sensor, including the real-time linear velocity. Then, the target distance is compared with a preset convergence radius, and the current speed information is compared with a preset speed threshold.

[0081] Furthermore, when the target distance is greater than the convergence radius, it indicates that the mobile robot has not yet reached the acceptance range of the current global sub-target node; when the target distance is continuously greater than the convergence radius within a preset time period, and the current speed information is continuously less than or equal to the speed threshold within the same preset time period, the mobile robot is determined to be in a deadlock state; when the target distance is less than or equal to the convergence radius, it is considered that the mobile robot has reached the current global sub-target node.

[0082] In one embodiment, based on A The algorithm generates a local sub-target sequence using the mobile robot's current position, the current global sub-target node, or any global sub-target node following the current global sub-target node. This includes: determining the search range in the grid map using the mobile robot's current position as the search starting point and the current global sub-target node or any global sub-target node following the current global sub-target node as the search ending point; and within the search range, using A... The algorithm searches for feasible paths and evaluates the actual cost of moving from the current node to an adjacent node and the estimated cost of moving from the adjacent node to the search endpoint during the search process. It selects the adjacent node that minimizes the sum of the actual cost and the estimated cost to expand the path, and expands it step by step until the search endpoint is reached, thus obtaining a local obstacle avoidance path composed of the expanded nodes. The nodes arranged in order in the local obstacle avoidance path are extracted as a local sub-objective sequence.

[0083] The search area is a rectangular or circular region centered on the mobile robot's current position and defined by a preset local planning radius. Furthermore, the search endpoint is configured to be located within this search area to ensure A... The algorithm can quickly complete path search within a limited computational overhead.

[0084] Actual cost represents the accumulated path length or movement consumption from the search starting point to the current node along the searched path. It reflects the travel cost that the mobile robot has already paid from the starting point to the current node. In a grid map, actual cost is usually expressed as the number of grid cells traversed from the starting point to the current node or the sum of weighted distances.

[0085] The estimated cost, calculated by a heuristic function, is the estimated path length or movement cost from the current node to the search endpoint. It guides the search direction, prioritizing expansion towards the endpoint. This heuristic function is configured to not exceed the actual minimum cost to ensure the optimal path is found. In this embodiment, the estimated cost is calculated using Manhattan distance, which is the sum of the horizontal and vertical grid cell counts between the current node and the search endpoint in the grid map.

[0086] Specifically, upon detecting that the mobile robot has entered a deadlock state, A is immediately initiated. The algorithm performs local path planning. Starting from the mobile robot's current position, it defines a local search range on the grid map, with the current global sub-target node or any subsequent global sub-target node as the search endpoint. Then, within this defined search range, it uses A... The algorithm searches for a feasible path from the starting point to the ending point of the search.

[0087] Furthermore, A The core search mechanism of the algorithm is as follows: Starting from the search starting point, a priority queue of nodes to be expanded is maintained. The priority of each node is determined by its total estimated cost, which is equal to the sum of the actual cost from the search starting point to the node and the estimated cost from the node to the search endpoint. In each iteration, the node with the smallest total estimated cost is selected from the priority queue for expansion. All neighboring nodes of this node are evaluated. For each neighboring node, if the node is within the search range and not in an obstacle area, the actual cost from the search starting point through the current node to the neighboring node is calculated, and the cost from the neighboring node to the search endpoint is also calculated. The estimated cost of the search endpoint and the sum of the estimated cost of the two adjacent nodes are the total estimated cost of the adjacent node. The adjacent node is added to the priority queue, and its parent node is recorded as the current node. The above expansion process is repeated, advancing towards the search endpoint step by step, until the search endpoint is removed from the priority queue for expansion or the maximum number of search steps is reached. At this point, starting from the search endpoint, the path can be traced back in reverse according to the recorded parent node pointer to obtain a continuous path from the search start point to the search endpoint. This path consists of a series of adjacent grid nodes located within the feasible area and avoids the obstacle area within the search range, which is the local obstacle avoidance path.

[0088] After obtaining the local obstacle avoidance path, the nodes in the path are extracted in the order of travel to form a local sub-target sequence. This sequence starts from the current position of the mobile robot and points to each key turning point on the local obstacle avoidance path in sequence, and finally ends at the search endpoint.

[0089] It should be noted that when the mobile robot is detected to be in a deadlock state and A is triggered... When the algorithm performs local replanning, it will then use the index of the current global sub-target sequence (e.g., ...). k Suspend, i.e., pause movement towards the current global sub-target node, but the position and order of that global sub-target node in the sequence remain unchanged; during local planning execution, the mobile robot moves along A The algorithm generates a sequence of local sub-targets, the endpoint of which is guaranteed to be within the feasible region of the global planner's cost map. Once the mobile robot reaches the vicinity of the endpoint of the local sub-target sequence and meets the recovery condition, the suspended index is retrieved. k Re-enter the global sub-target sequence as the first k Each node continues to move towards the target, and the index is only incremented when the mobile robot actually reaches that node. k +1, instead of skipping the current target node when local planning is triggered. Since the global sub-target sequence is pre-computed and statically stored, the index... k As a persistent pointer, it only increments when the global convergence condition is met, therefore A The insertion of the global sub-target sequence generated by the algorithm only changes the spatial trajectory of the mobile robot within a local range, but does not rearrange, skip, or omit global sub-target nodes that have not yet been reached, thus maintaining the topological order and integrity of the global sub-target sequence, i.e., A. The algorithm's local planning serves only as an intermediate transition and does not disrupt the topological order of the global sequence.

[0090] In this embodiment, when the mobile robot moves along the local sub-target sequence, A The algorithm's planned path guides it to gradually bypass the obstacles causing the deadlock, thus escaping the deadlock region. Because A The algorithm inspires the search direction and avoids blind expansion, thus enabling local path search to be completed in a very short time, meeting the computational speed requirements of real-time control.

[0091] S205, when the mobile robot meets the preset recovery conditions, control the mobile robot to move along the global sub-target nodes that have not been reached in the global sub-target sequence.

[0092] The implementation method of this step is the same as that of S105 above. For details, please refer to the above embodiments, which will not be repeated here.

[0093] It should be noted that during the switch between global and local planning, the quadratic planning controller with control obstacle function constraints remains active, performing safety filtering on the actual control input of the mobile robot. Since the boundary of the control obstacle function constraints depends only on the current continuous state of the mobile robot (e.g., position, orientation angle) and the geometric distribution of environmental obstacles, and is independent of the desired control input output by the upper-level planner, even if the desired control input undergoes discontinuous jumps when switching between global and local planning, the mobile robot's control system can still maintain the forward invariance of the safety set after this desired control input is projected onto the feasible control space determined by the control obstacle function constraints. That is, the distance between the mobile robot and all obstacles is always no less than the safety distance, and the safety is not interrupted by the switch of the upper-level planning logic. Simultaneously, to prevent chattering caused by frequent switching between global and local planning at the deadlock detection boundary, this application can also introduce a time hysteresis mechanism in the switching logic. Once A is triggered... The algorithm performs local planning, and the control system is locked in this local planning mode until the preset minimum dwell time is met before it is allowed to switch modes again. Since there is a clear lower bound between any two consecutive switching events, the maximum switching frequency of the control system is strictly limited, thereby effectively suppressing the occurrence of Zeno behavior and ensuring the stability and smoothness of the control system.

[0094] In summary, the two-stage safety control method for autonomous mobile robots provided in this application first generates a global path on a grid map using a DP algorithm and then optimizes it by redundancy removal to obtain a simplified global sub-target sequence. This provides macroscopic direction guidance for the mobile robot, enabling it to move along the globally optimal route in a wide range of environments. This avoids the deadlock problem caused by force balance in complex obstacle areas where local gradient information from a quadratic planning controller is relied upon. Simultaneously, it effectively shortens the travel distance and reduces energy consumption. Furthermore, when the mobile robot is in a deadlock state, it pauses tracking the current global sub-target node and switches to A. The algorithm performs local sub-objective sequence planning to quickly generate obstacle avoidance paths to guide the mobile robot out of the deadlock region. Due to A... The algorithm's local planning is initiated only on demand when deadlock is triggered, thus avoiding the time burden of continuous computation at high resolutions inherent in dynamic programming (DP) algorithms. Furthermore, once local planning is complete, tracking of unreached target nodes in the original global sub-target sequence resumes, ensuring both the topological invariance of the global path and real-time response to sudden dynamic obstacles. Additionally, the on-demand activation of deadlock detection and local planning allows the quadratic planning controller, based on control obstacle function constraints, to continuously output control commands that satisfy safety constraints with low computational cost for the vast majority of the time, intervening only in local replanning when necessary. This reduces the overall average computational load of the mobile robot system. Therefore, this application effectively overcomes the robot deadlock problem, reduces computational and travel energy consumption, and improves the operational efficiency and robustness of mobile robots in complex dynamic environments.

[0095] To verify the effectiveness of the two-stage safety control method for autonomous mobile robots proposed in this application, a multi-robot simulation experiment was conducted in a mixed environment containing both static and dynamic obstacles. Figure 5 As shown, Figure 5 An A provided for embodiments of this application A comparison of the computation time of the algorithm and the DP algorithm for path planning under the same grid width.

[0096] Depend on Figure 5 It can be seen that, under the same grid width, A The computation time of the algorithm is always less than that of the DP algorithm, and the computation time of the DP algorithm increases exponentially as the grid width decreases, but A... The algorithm's computation time remained consistently low, showing no significant increase with changes in grid width, indicating that A The algorithm can complete local path search in a very short time, and its calculation speed can match that of a real-time quadratic programming controller, thus meeting the real-time control requirements in dynamic obstacle environments.

[0097] Furthermore, such as Figure 6 As shown, Figure 6 This application provides a trajectory diagram of a mobile robot navigating an environment with both static and dynamic obstacles using this method, as part of an embodiment of the present application. Specifically, Figure 6 The first mobile robot 1, the second mobile robot 2, and the third mobile robot 3 in the diagram all have the same structure, and their initial physical attitude coordinates in space are as follows: , , Furthermore, each mobile robot is assigned the same destination task, namely, it must safely reach the target destination. Movement. In addition, the experimental environment included five fixed static obstacles (obstacle 601, 602, 603, 604, and 605) and three continuously randomly moving dynamic obstacles (obstacle 606, 607, and 608). The danger radius of obstacle 601 (the minimum permissible distance between the robot and the obstacle) was 0.7m; the danger radius of obstacle 602 was 0.7m; the danger radius of obstacle 603 was 0.7m; the danger radius of obstacle 604 was 0.4m; the danger radius of obstacle 605 was 0.5m; and the danger radii of obstacles 606, 607, and 608 were all 0.15m. Furthermore, the sampling interval was controlled at 0.033 seconds, the maximum linear velocity was 0.15 m / s, and the gain function in the obstacle control function was configured as follows: To prevent internal collisions between mobile robots, a minimum safe distance of 0.17 meters is set between them.

[0098] Under the above conditions, after controlling each mobile robot using the method provided in the embodiments of this application, the motion trajectories of the three mobile robots can be seen as follows: Figure 6 ,Depend on Figure 6 As can be seen, none of the three mobile robots fell into a deadlock state during the entire experiment. Each mobile robot maintained a safe distance from other mobile robots and from all surrounding static and dynamic obstacles. Furthermore, the adjustment trajectories to avoid dynamic obstacles were smooth and time-efficient. Therefore, the two-stage safety control method for autonomous mobile robots provided in this application effectively balances global pathfinding efficiency with underlying hardware safety assurance, achieving collaborative collision-free operation of multiple mobile robots in complex dynamic environments with low computational resource consumption.

[0099] This application also provides a computer device, which may specifically be a personal computer, a server, a network device, etc. Figure 7 As shown, the computer device includes a bus, a processor, memory, and a communication interface, and may also include an input / output interface and a display device. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores an operating system, computer programs, and a database. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The database stores location information. The network interface allows communication with external terminals via a network connection. When the computer program is executed by the processor, it implements the steps in the various method embodiments.

[0100] Those skilled in the art will understand that Figure 7 The structure shown is only a part of the structure related to the solution of this application and does not constitute a limitation on the computer device on which the solution of this application is applied. The specific computer device may include more or fewer components, or combine certain components, or have different component arrangements.

[0101] In one embodiment, a computer-readable storage medium is provided, which may be non-volatile or volatile, having stored thereon a computer program that, when executed by a processor, implements the steps in the above method embodiments.

[0102] In one embodiment, a computer program product is provided, including a computer program that, when executed by a processor, implements the steps in the above method embodiments.

[0103] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM). The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, graphics processors, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0104] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0105] The above embodiments are merely illustrative of several implementation methods of this application, and their descriptions are relatively specific and detailed. However, they should not be construed as limiting the scope of this application. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the protection scope of this application. Therefore, the protection scope of this application should be determined by the appended claims.

Claims

1. A two-stage safety control method for an autonomous mobile robot, characterized in that, include: Based on the grid map, the DP algorithm is used for global path planning to generate an initial global path. The grid map is used to represent the spatial layout information of the mobile robot's working environment. The initial global path is globally optimized to remove redundant nodes and generate a global sub-target sequence, which includes multiple global sub-target nodes arranged in order. The mobile robot is controlled to move along the global sub-target sequence, and the current global sub-target node in the global sub-target sequence is used as the tracking target. A quadratic programming controller based on the control obstacle function constraint generates the control input to drive the mobile robot, so that the mobile robot moves towards the current global sub-target node while maintaining a preset safe distance from the obstacle. The control obstacle function is used to constrain the distance between the mobile robot and the obstacle to be no less than the preset safe distance. When the mobile robot is detected to be in a deadlock state, it stops moving towards the current global sub-target node and, based on A... The algorithm uses the current position of the mobile robot, the current global sub-target node, or any global sub-target node after the current global sub-target node to generate a local sub-target sequence, and controls the mobile robot to move along the local sub-target sequence to escape the deadlock region. When the mobile robot meets the preset recovery conditions, the mobile robot is controlled to move along the global sub-target nodes that have not been reached in the global sub-target sequence.

2. The two-stage safety control method for autonomous mobile robots according to claim 1, characterized in that, The process of generating an initial global path based on a grid map and using a dynamic programming (DP) algorithm includes: Based on the grid map, the starting node, target node, and obstacle area of ​​the mobile robot are determined; Based on the starting node, the target node, and the obstacle region, the DP algorithm is used to perform global path planning to obtain the initial global path.

3. The two-stage safety control method for autonomous mobile robots according to claim 2, characterized in that, The initial global path is obtained by using the DP algorithm to perform global path planning based on the starting node, the target node, and the obstacle region, including: On the grid map, with the starting node as the starting point and the target node as the ending point, the obstacle area is mapped as an obstacle grid. The state space, action space, and state transition function of the mobile robot in the grid map are defined. The state space is the set of all non-obstacle grids. The state transition function is used to describe the position change rules of the mobile robot after performing any action. Set an immediate reward value for each action, and perform value iteration based on the state transition function and the immediate reward value to obtain the optimal value of each node in the grid map; Based on the optimal value of each node, the optimal node sequence from the starting node to the target node is obtained by backtracking, which serves as the initial global path.

4. The two-stage safety control method for autonomous mobile robots according to claim 2, characterized in that, The step of globally optimizing the initial global path, removing redundant nodes, and generating a global sub-target sequence includes: Extract the path points traversed by the initial global path and form an initial sub-target sequence in the order of travel. The initial sub-target sequence includes multiple initial sub-target nodes arranged in sequence. Using the current initial sub-target node in the initial sub-target sequence as the first endpoint and the initial sub-target node that is located after the current initial sub-target node and is farthest from the current initial sub-target node as the second endpoint, determine whether the line connecting the first endpoint and the second endpoint passes through the obstacle area. If the line passes through the obstacle area, the second endpoint is updated to the node whose index value is between the index of the first endpoint and the current index of the second endpoint, and the step of determining whether the line connecting the first endpoint and the second endpoint passes through the obstacle area is returned. If the target is not crossed, all intermediate nodes between the first endpoint and the second endpoint are identified as redundant nodes and deleted from the initial sub-target sequence. The second endpoint is updated to the new current initial sub-target node, and the process returns to the step of using the current initial sub-target node in the initial sub-target sequence as the first endpoint, until the current initial sub-target node is the last node in the initial sub-target sequence. The remaining nodes are then output in their original order to obtain the global sub-target sequence.

5. The two-stage safety control method for autonomous mobile robots according to claim 1, characterized in that, The quadratic programming controller based on control obstacle function constraints generates the control inputs to drive the mobile robot, including: Obtain the current location information of the mobile robot; Based on the positional deviation between the current position information and the current global sub-target node, the desired control input is calculated; Obtain the distance information between the mobile robot and the obstacle, and construct the obstacle control function based on the difference between the distance information and the safe distance; A quadratic programming problem is constructed using the desired control input as the objective and the control barrier function as the safety constraint. Solving the quadratic programming problem yields the actual control input that most closely approximates the desired control input while satisfying safety constraints, which serves as the control input for driving the mobile robot.

6. The two-stage safety control method for autonomous mobile robots according to claim 5, characterized in that, The expression for the control barrier function is: in, For safety functions, This refers to the current location information of the mobile robot. For safety functions Lie derivative along the drift state, The Lie derivative of the safety function along the control input matrix, Let be the control input variables to be solved in the quadratic programming problem. It is a monotonically increasing function. This refers to the center position information of the obstacle. For the Euclidean norm, The equivalent radius of the obstacle. The safe distance; The expression for the quadratic programming problem is: in, This is the actual control input. It is a positive definite weighted matrix. for m 3D real space, For the desired control input, This is a transpose operation.

7. The two-stage safety control method for autonomous mobile robots according to claim 1, characterized in that, The detection of the deadlock state includes: Obtain the current position information of the mobile robot and the target distance between the current global sub-target node, as well as the current speed information of the mobile robot; If the target distance is continuously greater than a preset convergence radius within a preset time period, and the current speed information is continuously less than or equal to a speed threshold within a preset time period, the mobile robot is determined to be in a deadlock state.

8. The two-stage safety control method for autonomous mobile robots according to claim 1, characterized in that, The A-based The algorithm generates a local sub-target sequence using the mobile robot's current position, the current global sub-target node, or any global sub-target node following the current global sub-target node, including: The search range is determined in the grid map with the current position of the mobile robot as the starting point and the current global sub-target node or any global sub-target node after the current global sub-target node as the search endpoint. Within the search range, A is used. The algorithm searches for feasible paths and evaluates the actual cost of moving from the current node to an adjacent node and the estimated cost of moving from the adjacent node to the search endpoint during the search process. It then selects the adjacent node that minimizes the sum of the actual cost and the estimated cost to expand the path. The path is expanded sequentially until the search endpoint is reached, thus obtaining a local obstacle avoidance path composed of the expanded nodes. Extract the nodes arranged in sequence in the local obstacle avoidance path to obtain the local sub-target sequence.

9. The two-stage safety control method for autonomous mobile robots according to claim 1, characterized in that, The recovery condition includes that the distance between the current position of the mobile robot and the end node of the local sub-target sequence is less than or equal to a preset distance.

10. A storage medium having a computer program stored thereon, characterized in that, When a computer program is executed by a processor, it implements the method of any one of claims 1 to 9.

Images