A robot control method and device based on a behavior tree

Abstract

The application discloses a robot control method and device based on a behavior tree, and belongs to the field of robot control. The application initializes a behavior tree by acquiring a work task instruction and constructs a shared state management space, determines each node corresponding to the current work and an execution sequence from a preset node library according to a work type, sequentially dispatches and executes each node according to the execution sequence after the mapping of each node to the behavior tree, each node acquires input data from the shared space and writes an execution state, continues to execute the next node until all are completed when successful, triggers a corresponding abnormal recovery strategy when failed, and controls the robot to safely exit when recovery fails. The application effectively solves the problem that the prior art cannot accurately and efficiently control the robot based on the behavior tree.

Description

Technical Field

[0001] This invention relates to the field of robot control, and in particular to a robot control method and apparatus based on behavior trees. Background Technology

[0002] Behavior Trees (BTs) originated in the computer game industry for modeling the behavior of non-player characters (NPCs). Due to their modularity, hierarchy, and high readability, they have been widely applied in the field of robot control in recent years. Compared to Finite State Machines (FSMs), Behavior Trees describe behavioral logic through structural control nodes such as sequential nodes, selection nodes, and parallel nodes, avoiding the problem of the number of states in a state machine increasing exponentially with the task scale. Furthermore, Behavior Trees have a built-in fallback mechanism implemented through selection nodes, allowing exception handling and error recovery to be implemented systematically through decorator nodes (such as retry nodes and timeout nodes), without needing to explicitly write recovery logic for each state transition. Based on these advantages, Behavior Trees have proven to be a scalable, maintainable, and formally semantic framework for robot task planning and control. In existing Behavior Tree implementations, nodes typically share state through a blackboard mechanism—a message passing capability that allows different nodes in the behavior tree to read and write state data. Nodes can write execution results to the blackboard, and subsequent nodes can read the required data from the blackboard.

[0003] However, existing behavior tree-based robot control schemes still have significant limitations in practical engineering applications. In complex scenarios involving the collaborative operation of multiple functional modules such as mobile chassis, robotic arms, 3D vision, lifting mechanisms, and voltage detection devices, existing solutions often treat the task organization function of the behavior tree and the data interaction mechanism between modules separately—the behavior tree is only responsible for arranging the execution order of nodes, while data transfer between modules is still accomplished through direct interface calls between modules. The blackboard is only used for temporary variable storage and does not truly become the sole channel for data interaction between modules. This hybrid architecture of "behavior tree scheduling + direct module communication" results in strong coupling between functional modules. When a functional module needs to be added, replaced, or modified, it is often necessary to simultaneously adjust the interfaces of other modules that have direct calling relationships with it, thus limiting the scalability and maintainability of the system. Furthermore, existing solutions for handling complex anomalies such as navigation failure replanning, visual recognition failure reacquisition and fine-tuning, robotic arm inverse kinematics solution failure attitude replanning, and safe exit after electrical detection anomalies typically still rely on hard-coded recovery logic within behavior tree nodes or state machines to manage anomaly transitions. This fails to fully leverage the structured advantages of behavior tree selection and retry nodes in anomaly handling. These shortcomings prevent existing technologies from accurately and efficiently controlling robots based on behavior trees. Summary of the Invention

[0004] This invention provides a robot control method and apparatus based on behavior trees to solve the problem that existing technologies cannot accurately and efficiently control robots based on behavior trees.

[0005] In a first aspect, this application provides a robot control method based on behavior trees, including: Obtain the robot's task instructions, initialize the behavior tree based on the task instructions, and construct a shared state management space based on the task information in the task instructions; Obtain a preset robot control node library, and determine the nodes corresponding to this task and the execution order of each node from the node library according to the task type in the task instruction. Each node is mapped to the behavior tree so that the robot can schedule the corresponding nodes sequentially according to the behavior tree and the execution order. For any node, the input data corresponding to the node is obtained from the shared state management space to control the robot to execute the task corresponding to the node and obtain the robot's execution state of the task corresponding to the node. When the execution status is successful, the robot is controlled to execute the next node corresponding to the node according to the behavior tree and the execution order, until all nodes corresponding to this task are executed, so that the robot can complete this task. When the execution status is failure, the abnormal recovery strategy corresponding to the node is triggered to complete the task corresponding to the node and continue to execute the next node corresponding to the node. If the robot fails to execute the node, the robot is controlled to exit safely.

[0006] This application constructs a shared state management space independent of each node. Based on the job type, it determines the node sequence corresponding to the current job from a pre-defined node library and maps this sequence to a behavior tree. This allows each node to obtain input data from this shared space before execution and write its execution state back to the shared space after execution. Data flow between nodes is entirely completed through this shared space without relying on direct calls between nodes. When adding, replacing, or modifying any node, there is no need to adjust the internal implementation or interface of other nodes, thus severing the direct dependencies between nodes, reducing system coupling, and improving system scalability and maintainability. Simultaneously, the behavior tree makes unified scheduling decisions based on the execution state in the shared space. Upon successful execution, it schedules the next node according to the execution order; upon failure, it triggers a response to the node's execution status. The proposed anomaly recovery strategy uses a structured approach to uniformly implement process connections and anomaly transitions between nodes at the behavior tree level. This replaces the traditional approach of relying on hard-coded internal nodes or additional state machines to manage anomaly branches. This allows each node to focus solely on its own operational logic without handling the details of process connections and anomaly recovery, reducing the cohesion complexity of each node and improving the readability of the task flow. Furthermore, when a node fails to execute, the behavior tree triggers the corresponding anomaly recovery strategy. Execution continues only after successful recovery; otherwise, the robot safely exits. This enables the system to perform targeted recovery operations based on the source of the anomaly without implementing recovery logic individually within each node. This reduces inter-node coupling and node cohesion complexity while improving the overall robustness of the system in handling various execution anomalies. This application effectively solves the problem that existing technologies cannot accurately and efficiently control robots based on behavior trees.

[0007] Furthermore, determining the nodes corresponding to this task and their execution order from the node library based on the task type in the task instruction specifically involves: When the operation type is installation operation, the nodes corresponding to this operation are determined as the safety pre-inspection node, chassis global navigation node, lifting and coarse positioning node, local three-dimensional vision recognition node, coordinate unified transformation node, voltage detection condition judgment node, robotic arm local vision servo node and grounding wire installation node. When the operation type is demolition operation, the nodes corresponding to this operation are determined as the safety pre-inspection node, chassis global navigation node, lifting and coarse positioning node, local three-dimensional vision recognition node, coordinate unified transformation node, voltage detection condition judgment node, robotic arm local vision servo node and demolition node. For any of the job types, the execution order is the sequential order of the nodes mentioned above within that job type.

[0008] This application matches the complete node sequence corresponding to the job type from a preset node library based on the job type, so that the installation and dismantling operations each have a complete execution path from the safety pre-check to the final operation node. The two types of operations share the pre-check nodes such as safety pre-check, chassis navigation, lifting and coarse positioning, visual recognition, coordinate transformation, voltage detection and judgment and robotic arm servo. They are only distinguished at the final operation node by the grounding wire installation node and dismantling node. Thus, different types of operation tasks are supported with the same behavior tree structure without the need to build a separate behavior tree for each job type, which reduces the configuration complexity and maintenance cost of the behavior tree. Meanwhile, by mapping the job type to the node sequence, the behavior tree can obtain the complete execution order before running, eliminating the need to dynamically determine the type or direction of subsequent nodes based on the node execution results during operation. This simplifies the scheduling logic of the behavior tree and improves the execution efficiency of the system during operation. In addition, when a new job type needs to be added, it is only necessary to configure the corresponding node sequence for the job type in the node library to access the behavior tree for scheduling. There is no need to modify the original scheduling framework of the behavior tree and the logical relationship between nodes, thereby enhancing the scalability of the system and enabling the robot to adapt to more types of job tasks without increasing the complexity of the scheduling logic.

[0009] Furthermore, the step of obtaining the input data corresponding to the node from the shared state management space to control the robot to execute the task corresponding to the node specifically involves: The shared status management space stores the job type, target equipment number, map data, chassis pose, lifting height, visual recognition pose, robotic arm target pose, visual confidence level, voltage detection result, safety status, and job result. When the node is a safety pre-check node, the robot is controlled to obtain the safety status from the shared state management space, and the safety status is verified to obtain the verified execution status; the verified execution status is written into the shared state management space. When the node is a chassis global navigation node, the robot is controlled to obtain the target device number and the map data from the shared state management space, and the target device position is determined according to the target device number. The robot's chassis is then controlled to move to the coarse positioning area near the target device to obtain the chassis pose and navigation state after the movement. The chassis pose and navigation state after the movement are then written into the shared state management space. When the node is a lifting and coarse positioning node, the robot is controlled to obtain the chassis pose from the shared state management space, so as to control the robot's built-in lifting mechanism to adjust the height to enter the detection range of the robot's built-in vision sensor, obtain the adjusted lifting height, and write the adjusted lifting height into the shared state management space. When the node is a local 3D visual recognition node, the robot is controlled to obtain the chassis pose and lifting height from the shared state management space to identify the task target of this operation and output the visual recognition pose and visual confidence score, and write the visual recognition pose and visual confidence score into the shared state management space. When the node is a coordinate unified transformation node, the robot is controlled to obtain the visual recognition pose from the shared state management space, convert the visual recognition pose into a target pose that the robot's robotic arm can execute, and write the target pose into the shared state management space. When the node is a voltage testing condition judgment node, the robot is controlled to obtain the voltage testing result from the shared state management space, and the voltage testing result is judged to obtain the judgment execution state; the judgment execution state is written into the shared state management space. When the node is a local vision servo node of the robotic arm, the robot is controlled to obtain the target pose of the robotic arm, the electrical detection result and the safety status from the shared state management space, so as to control the robotic arm to perform the corresponding operation and obtain the robotic arm status and operation result; the robotic arm status and operation result are written into the shared state management space.

[0010] This application unifies the storage of job type, target equipment number, map data, chassis pose, lifting height, visual recognition pose, robotic arm target pose, visual confidence level, voltage detection result, safety status, and job result in a shared state management space. Safety pre-check nodes, chassis global navigation nodes, lifting and coarse positioning nodes, local 3D visual recognition nodes, coordinate transformation nodes, voltage detection condition judgment nodes, and robotic arm local visual servo nodes all obtain their required input data from this shared space. After execution, they write the execution status and job result back to this shared space. This ensures that the data required by any node during execution comes from the same data space, and the execution result is uniformly written back to this data space. Nodes no longer need to know each other's internal data structures or call each other's input / output interfaces; data transmission and state synchronization can be completed solely through this shared state management space. This eliminates direct dependencies between nodes. When modifying the internal processing logic of any node, only its read / write data fields need to remain unchanged to avoid affecting the normal operation of other nodes. When adding or replacing nodes in the job flow, only the corresponding data fields read and written from the shared space need to be configured for that node. Without needing to adjust the code implementation of existing nodes, the coupling degree and maintenance cost of the system are significantly reduced. At the same time, the chassis pose written by the chassis global navigation node is read by the lifting and coarse positioning node to determine the lifting reference. The lifting height written by the lifting and coarse positioning node and the chassis pose are read by the local 3D vision recognition node to determine the effective field of view of vision acquisition. The vision recognition pose written by the local 3D vision recognition node is read by the coordinate unification transformation node to perform coordinate system transformation. The robot arm target pose written by the coordinate unification transformation node and the execution status written by the voltage detection condition judgment node are read by the robot arm local vision servo node to perform the final operation. This hierarchical data-driven relationship from navigation to vision to robot arm realizes the implicit connection between the input and output of nodes through the shared state management space. The execution order of each node is implicitly determined by its own read and write data dependency relationship. The behavior tree only needs to schedule the nodes in sequence according to the preset operation process to ensure that the data required by each node has been written to the shared space by the previous node when it is executed. There is no need to explicitly transfer data or wait for synchronization signals between nodes, thereby simplifying the collaboration mechanism between nodes and reducing the system adjustment cost caused by changes in the data transmission link.

[0011] Furthermore, the scheduling process for the job corresponding to the safety pre-check node is as follows: When the node is a safety pre-check node, the robot is controlled to obtain the safety status from the shared state management space; the safety status includes emergency stop status, communication status, electrical safety status, and robot arm initial posture; When the emergency stop state is not triggered, the communication state is within a preset first normal range, the electrical safety state is within a preset second normal range, and the robot arm's initial posture is within a preset safety range, a successfully verified execution state is generated and written into the shared state management space. Otherwise, an execution state indicating a verification failure is generated and written to the shared state management space, and an anomaly recovery strategy corresponding to the security pre-check node is triggered; the anomaly recovery strategy is to control the robot to enter a safe exit state.

[0012] This application addresses the issue of storing four types of safety status data—emergency stop, communication, electrical safety, and robot arm initial posture—in a unified shared state management space. Before the start of the operation, a safety pre-check node centrally retrieves all safety statuses from this shared space and performs unified verification. Compared to traditional solutions where safety checks are scattered across various work nodes or only partially checked at specific stages, this approach ensures that all safety conditions are centrally verified before the robot performs any substantive actions. The verification results are then used to generate an execution state and written into the shared state management space for all subsequent nodes to perceive. The behavior tree then determines the execution state based on this verification result. The system determines whether to continue the work process or directly enter a safe exit state, thus decoupling safety verification from work execution into an independent pre-stage. This avoids the risk of missed detections or the overhead of repeated verification caused by embedding safety detection logic in various nodes. In addition, when any safety condition is not met, the verification immediately fails and triggers a safe exit, ceasing subsequent chassis movement, visual recognition, or robotic arm operations. This prevents potential equipment damage or personal injury from the robot continuing to perform work when an emergency stop has been triggered, communication is abnormal, electrical is abnormal, or the robotic arm posture is unsafe, thereby improving the robot's operational safety in complex working environments.

[0013] The scheduling process for the job corresponding to the chassis global navigation node is as follows: When the node is a chassis global navigation node, the robot is controlled to obtain the target device number and the map data from the shared state management space; The location of the target device is determined based on the target device number, and a global path from the robot's current location to the coarse positioning area is planned based on the map data; The robot's chassis is controlled to move along the global path, and the chassis is monitored in real time to see if it reaches the coarse positioning area during the movement. When the chassis reaches the coarse positioning area, the chassis pose and navigation success status after the movement are obtained, and the chassis pose and navigation success status after the movement are written into the shared state management space. When the chassis fails to reach the coarse positioning area, the navigation status is determined to be abnormal. When the global path planning fails or the navigation status is abnormal, the navigation failure status is written to the shared status management space, triggering the abnormal recovery strategy corresponding to the global navigation node of the chassis. The abnormal recovery strategy is to replan the global path or control the chassis to perform local obstacle avoidance.

[0014] This application utilizes a chassis-based global navigation node to obtain the target device number and map data from a shared state management space before execution. Based on the target device number, the node determines the target device's location and plans a global path from the current location to the coarse positioning area. The chassis is then controlled to move along this path, with real-time monitoring to ensure it reaches the coarse positioning area. Upon arrival, the chassis pose and navigation success status are written to the shared state management space, allowing navigation results to be stored in a standardized data format for subsequent nodes. This decouples the navigation phase from subsequent operation phases. Furthermore, when path planning fails or the navigation status becomes abnormal, the system writes the failure status to the shared state management space and triggers a corresponding anomaly recovery strategy. Navigation is attempted to recover by replanning the global path or controlling the chassis to perform local obstacle avoidance, rather than directly terminating the task or simply retrying within the node after detecting an anomaly. This allows the system to autonomously select a recovery method matching the anomaly type when navigation is obstructed, thereby improving the robot's navigation robustness and task completion rate in complex operating environments.

[0015] The scheduling process for the jobs corresponding to the lifting and coarse positioning nodes is as follows: When the node is a lifting and coarse positioning node, the robot is controlled to obtain the chassis pose from the shared state management space; The robot's current position is determined based on the chassis pose, and the target lifting height is determined based on a preset lifting height threshold. The robot's built-in lifting mechanism is controlled to adjust from its current position to the target lifting height, and the encoder feedback value of the lifting mechanism is obtained in real time as the lifting height during the adjustment process; When the lifting height reaches the target lifting height, the adjusted lifting height is obtained and written into the shared state management space; When the lifting height cannot reach the target lifting height or the encoder feedback is abnormal, the abnormal recovery strategy corresponding to the lifting and coarse positioning node is triggered. The abnormal recovery strategy is to re-execute the lifting adjustment or control the robot chassis to make fine adjustments. If the readjustment or fine adjustment fails, the robot is controlled to enter the safe exit state.

[0016] This application sets the lifting and coarse positioning nodes as independent nodes. These nodes acquire the chassis pose from the shared state management space, determine the robot's current position based on this pose, and then determine the target lifting height according to a preset lifting height threshold. The lifting mechanism is then controlled to adjust to this target height, and encoder feedback values ​​are acquired in real-time during the adjustment process and written into the shared state management space as the lifting height. The lifting adjustment results are stored in the shared space as standardized data of encoder measurements for use by subsequent local 3D vision recognition nodes. Since there is no direct calling relationship between the lifting and coarse positioning nodes and the local 3D vision recognition nodes, the output data of the lifting node only needs to be written to the shared space without needing to know about subsequent nodes. By defining the data format or interface for the points, the visual nodes only need to read the lifting height from the shared space without needing to pay attention to the control details of the lifting mechanism, thus decoupling the lifting adjustment and visual recognition into two independent processing stages. At the same time, by obtaining the encoder feedback value in real time as the lifting height instead of directly using the target lifting height, the lifting height data written to the shared state is the actual position reached by the lifting mechanism rather than the theoretical command value. This ensures that the data used by the subsequent visual nodes when adjusting the field of view based on the lifting height is consistent with the actual physical state of the lifting mechanism, avoiding inaccurate field of view adjustment caused by the lifting mechanism's positioning deviation or mechanical return error, thereby improving the positioning accuracy before visual recognition and the effectiveness of visual recognition.

[0017] Furthermore, the scheduling process for the job corresponding to the local 3D visual recognition node is as follows: When the node is a local 3D vision recognition node, the robot is controlled to obtain the chassis pose and the lifting height from the shared state management space; Based on the chassis pose and the lifting height, the robot's built-in three-dimensional vision sensor collects color and depth images of the target object for this operation. The target of the current task is identified from the color image using a target detection algorithm, and the two-dimensional bounding box of the target in the image is output. Based on the two-dimensional bounding box and the depth image, the three-dimensional coordinates of the target in the visual coordinate system are calculated as the visual recognition pose. The visual confidence level is determined according to the target detection algorithm, and the visual recognition pose and the visual confidence level are written into the shared state management space; When the visual confidence level is lower than the preset confidence threshold or the task target for this task fails to be identified, the anomaly recovery strategy corresponding to the local 3D visual recognition node is triggered.

[0018] This application enables local 3D vision recognition nodes to acquire chassis pose and lifting height from a shared state management space before execution. Based on these two data points, the 3D vision sensor acquires color and depth images. A target detection algorithm identifies the target from the color image and outputs a 2D bounding box. The depth image is then combined to calculate the target's 3D coordinates in the visual coordinate system as the visual recognition pose. The visual confidence level is determined based on the target detection algorithm, and the visual recognition pose and confidence level are written into the shared state management space. This allows subsequent coordinate transformation nodes to directly read the visual recognition pose from the shared space for coordinate transformation without needing to concern themselves with the specific implementation details of the visual recognition. Since the vision node, navigation node, and lifting node only exchange data through the shared space without direct calling relationships, changes in the vision sensor model, installation location, or target detection algorithm only require modification of the vision node's internal implementation while keeping the data fields written to the shared space unchanged. Other nodes remain unaffected, thus reducing the coupling between nodes and improving system maintainability. Simultaneously, through target detection... The algorithm outputs a 2D bounding box, which is then combined with a depth image to calculate 3D coordinates. This accurately maps the visual recognition results from the 2D image space to the 3D physical space, providing accurate spatial pose data for the robotic arm to perform its tasks. This allows subsequent coordinate transformations and robotic arm control to be based on actual 3D coordinates rather than image pixel positions, avoiding mapping errors from 2D information to 3D space and improving the accuracy of the robotic arm's end effector reaching the target position. Furthermore, the confidence threshold is pre-set based on the target detection algorithm's accuracy and operational safety requirements. It distinguishes between reliable and unreliable recognition results. When the visual confidence is below this threshold, it indicates insufficient credibility of the recognition result, requiring re-acquisition and re-recognition. When the visual confidence is below the preset confidence threshold or recognition fails, a corresponding anomaly recovery strategy is triggered. This allows the system to autonomously trigger recovery operations when visual quality is poor or the target is occluded, rather than passing low-quality recognition results to subsequent nodes and causing task failure. This ensures that the target pose data used by the robotic arm before performing tasks has sufficient credibility, thereby improving the safety and reliability of task execution.

[0019] Furthermore, the scheduling process for the corresponding job of the coordinate transformation node is as follows: When the node is a coordinate unified transformation node, the robot is controlled to obtain the visual recognition pose from the shared state management space. The visual recognition pose is the pose of the task target in the visual coordinate system for this task. Based on preset hand-eye calibration parameters, the pose in the visual coordinate system is converted to the pose in the end-effector coordinate system of the robotic arm; Based on the transformation relationship between the robot's various coordinate systems, the pose in the end-effector coordinate system of the robotic arm is transformed to the pose in the base coordinate system of the robotic arm to obtain the target pose that the robotic arm can execute. Write the target pose into the shared state management space; When the visual recognition pose is invalid, the hand-eye calibration parameters fail to load, or the pose transformation from the end-effector coordinate system to the base coordinate system fails, an anomaly recovery strategy corresponding to the coordinate transformation node is triggered. The anomaly recovery strategy involves re-acquiring the visual recognition pose from the shared state management space and performing the coordinate transformation again. If the coordinate transformation fails again, the local 3D visual recognition node is triggered to re-perform visual recognition. If the transformation still fails after re-performing visual recognition, the robot is controlled to enter a safe exit state.

[0020] This application sets the coordinate unification transformation node as an independent node, enabling it to obtain the visual recognition pose in the visual coordinate system from the shared state management space. First, it transforms the pose to the robot arm's end effector coordinate system based on hand-eye calibration parameters. Then, based on the transformation relationships between the robot's various coordinate systems, it transforms the pose in the end effector coordinate system to the robot arm's base coordinate system. The resulting executable target pose is then written into the shared state management space. This ensures that the visual recognition result, after two stages of transformation (hand-eye calibration and coordinate system transformation), is ultimately unified to the robot arm's base coordinate system. Since the visual coordinate system, the robot arm's end effector coordinate system, and the robot arm's base coordinate system each exist independently and have different origin positions and axial definitions, the three-dimensional coordinates directly output by the visual sensor cannot be directly used for motion control by the robot arm. The independent processing of the coordinate unification transformation node gradually maps the visual recognition result from the visual coordinate system to the robot arm's base coordinate system, eliminating the limitations imposed by sensor installation location, robot arm... The coordinate deviations introduced by the joint configuration and the height changes of the lifting platform provide an accurate execution benchmark for the robotic arm's motion planning. Simultaneously, the transformed target pose is written into a shared state management space for the robotic arm's local vision servo nodes to read. This allows vision nodes to complete recognition without needing to know the robotic arm's control method or interface definition, and transformation nodes to not need to know the specific visual recognition process or the robotic arm's execution logic. Each node only transmits data through the shared space without direct calling relationships. When hand-eye calibration parameters change, only the parameter configuration of the transformation nodes needs to be modified, while the vision nodes and robotic arm nodes remain unaffected. When the robotic arm's installation position or configuration changes, only the transformation parameters of the corresponding coordinate system in the coordinate transformation relationship need to be adjusted without modifying the visual recognition logic or the robotic arm control logic. This ensures pose transformation accuracy while reducing the coupling between nodes, improving system maintainability and adaptability to different hardware configurations.

[0021] Furthermore, the scheduling process for the tasks corresponding to the local visual servo nodes of the robotic arm is as follows: When the node is a local vision servo node of the robotic arm, the robot is controlled to obtain the target pose of the robotic arm, the electrical detection result, and the safety status from the shared state management space; The target angles of each joint of the robotic arm are calculated by calling a preset inverse kinematics solver based on the target pose of the robotic arm. The robot's robotic arm is controlled to move to the target pose of the robotic arm according to the target angle of each joint, and the end effector of the robotic arm is controlled to perform clamping, alignment, installation or removal operations. The contact force feedback of the end effector is monitored during the execution of the robotic arm's end effector; When the robotic arm reaches the target pose and the clamping, alignment, installation or dismantling operation is completed, the robotic arm operation success status and operation completion result are obtained, and the robotic arm operation success status and operation completion result are written into the shared status management space. When the target pose of the robotic arm exceeds the workspace of the robotic arm or the calculation of the target angle of each joint of the robotic arm fails, an anomaly recovery strategy corresponding to the local visual servo node of the robotic arm is triggered. The anomaly recovery strategy is to first execute a preset end-effector posture replanning scheme. If the execution of the end-effector posture replanning scheme fails, the robot's chassis is controlled to make fine adjustments and the chassis global navigation node and the local three-dimensional visual recognition node are re-executed.

[0022] This application enables the robotic arm's local vision servo node to simultaneously acquire data on three dimensions—target pose, electrical verification result, and safety status—from a shared state management space before execution. This ensures that before the robotic arm starts moving, it has already confirmed that the target pose has been written to the shared space by the coordinate transformation node, the electrical verification condition has been confirmed by the electrical verification judgment node, and the safety status has been verified as normal by the safety pre-check node. Since these three data points originate from the processing results of three different nodes and are all converged to the robotic arm node through the shared state management space, the robotic arm node does not need to separately call the interfaces of the electrical verification module, safety module, or coordinate transformation module. It can obtain all the necessary pre-condition data for execution in a single read. This achieves unified data convergence at the data level for the originally scattered steps of electrical verification, safety verification, and robotic arm motion control through the shared space. This eliminates the need for the behavior tree to set multiple condition nodes in the tree structure to verify whether each pre-condition is met before scheduling the robotic arm node. The complexity of conditional branching in the behavior tree is reduced. After acquiring the above three data items, the robotic arm node calls the inverse kinematics solver to calculate the target angles of each joint based on the target pose and controls the robotic arm to move to the target pose, enabling the end effector to perform clamping, alignment, installation, or removal operations. During the execution process, the contact force feedback of the end effector is monitored, allowing the system to superimpose force perception capabilities on top of position control. When the end effector contacts the target, it can sense whether it is in position or whether it has collided with an obstacle based on the change in contact force, thereby avoiding damage to the robotic arm or the target caused by rigid collisions due to pure position control, and improving the compliance and safety of the robotic arm operation. When the robotic arm reaches the target pose and the operation is completed, the successful operation status and operation completion result of the robotic arm are written into the shared state management space, so that the final operation result is stored in the shared space in a standardized data form for the state feedback node to read, realizing a closed loop from data input to execution feedback.When the target pose exceeds the robot arm's workspace or the inverse kinematics solution fails, the behavior tree triggers the anomaly recovery strategy corresponding to that node. The robot arm's local vision servo node first executes the end-effector pose replanning scheme instead of directly reporting an error and exiting. It attempts to find a reachable target pose within the current chassis and lifting position by adjusting the end-effector's approach posture. Only after the end-effector pose replanning fails does it control the chassis for fine-tuning and re-execute global chassis navigation and local 3D vision recognition to obtain a new target pose. This allows the system to attempt recovery in a progressively degraded manner when robot arm motion planning fails—prioritizing adjustments to the robot arm's own posture, and then reconstructing the target pose by adjusting the robot's overall position after posture adjustment fails. Because the anomaly recovery process is structured into multiple alternative schemes by the behavior tree's selection nodes, rather than having the recovery logic hard-coded within the robot arm nodes, when new recovery methods are needed, only new alternative sub-nodes need to be added to the corresponding selection nodes in the behavior tree, without modifying the robot arm nodes' internal code. This improves the robot arm's operational success rate and safety while enhancing the system's scalability and maintainability in handling inverse kinematics solution anomalies.

[0023] Secondly, this application provides a robot control device based on behavior trees. The behavior tree-based robot control device includes: The first acquisition module is used to acquire the robot's task instructions, initialize the behavior tree according to the task instructions, and construct a shared state management space based on the task information in the task instructions. The second acquisition module is used to obtain a preset robot control node library. Based on the job type in the job task instruction, it determines the nodes corresponding to the current job and the execution order of the nodes from the node library. The control module is used to map each node to the behavior tree so that the robot can schedule the corresponding nodes in sequence according to the behavior tree and the execution order. For any node, the module obtains the input data corresponding to the node from the shared state management space to control the robot to execute the task corresponding to the node and obtain the robot's execution status of the task corresponding to the node. The execution module is used to control the robot to execute the next node corresponding to the current node according to the behavior tree and the execution order when the execution status is successful, until all nodes corresponding to the current task are executed, so that the robot can complete the current task. The recovery module is used to trigger the abnormal recovery strategy corresponding to the node when the execution status is failure, so as to continue to execute the next node corresponding to the node after completing the task corresponding to the node. If the robot fails to execute the node, the robot is controlled to exit safely.

[0024] This application acquires job task instructions through a first acquisition module and initializes a behavior tree and constructs a shared state management space based on these instructions. A second acquisition module determines the nodes and execution order corresponding to the current job from a pre-set node library based on the job type. A control module then maps each node to the behavior tree, causing the behavior tree to schedule the corresponding nodes sequentially according to the execution order. Each node acquires input data from the shared space before execution and writes its execution status to the shared space after execution. The execution module then determines whether the current node has succeeded based on the execution status in the shared space—if successful, it continues scheduling the next node until all nodes are completed; if unsuccessful, the recovery module triggers the corresponding exception recovery strategy. Because the modules exchange data through the shared state management space rather than directly calling each other's interfaces, the shared space constructed by the first acquisition module provides a unified data interaction center for all subsequent modules. The scheduling decisions of the control module and the process advancement of the execution module both rely on the data in the shared space. Instead of direct communication between modules, the recovery module only needs to trigger the exception recovery strategy based on the execution status in the shared space without needing to know the internal implementation of the execution module. This achieves loose coupling at the data level between modules. When it is necessary to modify the internal logic of a module, it is only necessary to keep its read and write data format to the shared space unchanged so as not to affect the normal operation of other modules. When it is necessary to add or replace nodes in the work process, it is only necessary to adjust the node sequence determined by the second acquisition module from the node library and the corresponding node read and write data configuration without modifying the scheduling framework of the behavior tree or the interface definition between modules. This reduces the coupling degree and maintenance cost of the system. At the same time, through the cooperation of each module, the robot can continue to advance the work according to the established process when the node execution is successful, and can trigger the exception recovery strategy to try to recover when the node execution fails, rather than directly terminating or falling into an undefined state. This improves the system's autonomous operation capability and overall robustness in complex work scenarios. Attached Figure Description

[0025] Figure 1 : A schematic flowchart of an embodiment of the behavior tree-based robot control method provided in this application; Figure 2 : A schematic diagram of an embodiment of the control flow of the behavior tree provided in this application; Figure 3 : A schematic diagram of an embodiment of the behavior tree-based robot control device provided in this application. Detailed Implementation

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

[0027] Example 1 Please refer to Figure 1 In order to solve the problem that existing technologies cannot accurately and efficiently control robots based on behavior trees, this invention provides a robot control method based on behavior trees, including steps S01-S05.

[0028] S01: Obtain the robot's task instructions, initialize the behavior tree according to the task instructions, and construct a shared state management space based on the task information in the task instructions; In a preferred embodiment of this invention, the steps of obtaining the robot's task instructions, initializing the behavior tree based on the task instructions, and constructing a shared state management space based on the task information in the task instructions are as follows: In one embodiment, the robot first obtains a task instruction from the remote control system. This task instruction includes at least the task type and target device number. The task type instructs the robot to perform a grounding wire installation or removal operation, and the target device number indicates the target device for this task. The robot initializes a behavior tree based on the obtained task instruction. Specifically, the robot reads a pre-written YAML configuration file using a custom DSL compiler and compiles the YAML configuration into a behavior tree structure. This behavior tree uses the py_trees library as its core framework and integrates with the ROS2 system using py_trees_ros. The compiled behavior tree contains multiple predefined node types, parameter passing relationships between nodes, and subtree reference relationships. After the behavior tree initialization is complete, the robot establishes a task context for the behavior tree. This task context records task-level information during the task execution process, including the task type, target device number, execution status of each node, and data passing relationships between nodes. The task type and target device number from the task instruction are written as task information into a preset shared state management space. This shared state management space is implemented using the py_trees blackboard module, serving as a unified channel for data interaction among the nodes of the behavior tree. During execution, each node writes the execution result to this shared state management space using the blackboard module's `set` method and retrieves the required input data from it using the blackboard module's `get` method. This ensures that data flow between nodes is entirely completed through this shared state management space, without relying on direct calls or message passing between nodes. After the behavior tree is initialized, it begins scheduling each node to execute sequentially according to a preset job flow. The default tick cycle of the behavior tree is set to 10Hz, meaning that the behavior tree node state update and scheduling loop is executed 10 times per second.

[0029] S02: Obtain a preset robot control node library, and determine the nodes corresponding to this task and the execution order of each node from the node library according to the task type in the task instruction. In a preferred embodiment of this invention, the step of obtaining a preset robot control node library and determining the nodes corresponding to the current task and their execution order from the node library according to the task type in the task instruction specifically involves: In one embodiment, the robot acquires a pre-built robot control node library. This library is a pre-built collection containing all executable control nodes of the robot, where each control node is pre-registered in the behavior tree framework via a registration mechanism. Specifically, the robot reads a pre-written YAML configuration file through the bt_dsl compiler. This configuration file defines the type of each node, the parameter passing relationships between nodes, and the subtree reference relationships. The bt_dsl compiler recursively expands subtrees using tree_compiler.py, parses parameters using param_resolver.py, and instantiates the corresponding behavior class from the registry based on the type field using node_factory.py. All custom behavior nodes are registered using the @register_behaviour decorator and inherit from the py_trees.behaviour.Behaviour base class, implementing the setup, initialize, update, and terminate lifecycle methods, thereby constructing the pre-built robot control node library.

[0030] After acquiring the preset robot control node library, the robot determines the corresponding nodes and their execution order from the library based on the job type in the task instruction. Specifically, the job types in the task instruction include installation and dismantling operations. When the job type is installation, the robot determines the following nodes from the node library: safety pre-check node, chassis global navigation node, lifting and coarse positioning node, local 3D vision recognition node, coordinate unified transformation node, voltage detection condition judgment node, robotic arm local vision servo node, and grounding wire installation node. When the job type is dismantling, the robot determines the following nodes from the node library: safety pre-check node, chassis global navigation node, lifting and coarse positioning node, local 3D vision recognition node, coordinate unified transformation node, voltage detection condition judgment node, robotic arm local vision servo node, and dismantling node. The order in which these nodes are arranged is the execution order; that is, the behavior tree schedules the nodes to be executed sequentially from front to back according to this arrangement.

[0031] In this process, installation and dismantling operations share the same pre-safety inspection node, chassis global navigation node, lifting and coarse positioning node, local 3D vision recognition node, coordinate unified transformation node, electrical condition judgment node, and robotic arm local vision servo node. They are only distinguished at the final operation node by the grounding wire installation and dismantling node, thus supporting different types of tasks with the same behavior tree structure. Through the mapping relationship between operation type and node sequence, the behavior tree can obtain the complete execution order before operation, eliminating the need to dynamically determine the type or direction of subsequent nodes based on the node execution results during operation, simplifying the behavior tree's scheduling logic.

[0032] S03: Map each node to the behavior tree so that the robot can schedule the corresponding nodes in sequence according to the behavior tree and the execution order. For any node, obtain the input data corresponding to the node from the shared state management space to control the robot to execute the task corresponding to the node and obtain the robot's execution status for the task corresponding to the node. In a preferred embodiment of this invention, the nodes are mapped to the behavior tree so that the robot sequentially schedules the corresponding nodes according to the behavior tree and the execution order. For any given node, the input data corresponding to that node is obtained from the shared state management space to control the robot to execute the task corresponding to that node, thereby obtaining the robot's execution state for the task corresponding to that node. Specifically: In one embodiment, after completing the behavior tree initialization and shared state management space construction, the robot maps each node corresponding to the current task to the behavior tree. Specifically, the robot uses the bt_dsl compiler to compile the node type, parameter passing relationship, and subtree reference relationship of each node into a behavior tree structure. All custom behavior nodes are registered through the @register_behaviour decorator and inherit the py_trees.behaviour.Behaviour base class, implementing the setup, initialize, update, and terminate lifecycle methods. This maps the safety pre-check node, chassis global navigation node, lifting and coarse positioning node, local 3D vision recognition node, coordinate unified transformation node, voltage detection condition judgment node, robotic arm local vision servo node, and grounding wire installation or removal node to scheduleable execution units in the behavior tree. After mapping, the behavior tree begins to schedule the corresponding nodes to execute sequentially from front to back according to the previously determined execution order. The scheduling mechanism of behavior trees is based on sequential nodes. That is, after the current node is executed successfully, the behavior tree automatically moves to the next node in the execution sequence until all nodes in the execution sequence have been executed or a node fails to execute, triggering an exception recovery strategy.

[0033] For any node in the execution sequence, before it is scheduled by the behavior tree and begins execution, it first obtains the input data required for its execution from the shared state management space. Specifically, the shared state management space is implemented using the py_trees blackboard module. Each node reads the corresponding input data from this shared state management space through the get method of the blackboard module. The specific data read by each node is as follows: The safety pre-check node obtains the safety status from the shared state management space, which includes the robot's emergency stop status, communication status, electrical safety status, and the robot arm's initial posture; the chassis global navigation node obtains the target device number and map data from the shared state management space; the lifting and coarse positioning node obtains the chassis pose from the shared state management space; the local 3D vision recognition node obtains the chassis pose and lifting height from the shared state management space; the coordinate unification transformation node obtains the visual recognition pose from the shared state management space; the voltage testing condition judgment node obtains the voltage testing result from the shared state management space; and the robot arm local vision servo node obtains the robot arm target pose, voltage testing result, and safety status from the shared state management space. After each node obtains the corresponding input data from the shared state management space, the robot controls that node to execute its corresponding task. Taking the chassis global navigation node as an example, before execution, this node obtains the target device number and map data from the shared state management space. Then, it determines the target device's location based on the target device number and plans a global path from the robot's current position to the coarse positioning area near the target device based on the map data. Next, it controls the robot's chassis to move along this global path, and during the movement, it detects in real time whether the chassis has reached the coarse positioning area, and finally obtains the chassis pose and navigation status after the movement.

[0034] After each node performs its corresponding task, it generates an execution state, which includes success and failure states. For example, when the safety pre-check node passes the safety verification, the verification result is the execution state corresponding to that verification; when the chassis global navigation node successfully reaches the coarse positioning area, the navigation success state is the execution state corresponding to navigation; when the robotic arm local vision servo node successfully reaches the target pose and completes the operation, the robotic arm operation success state is the execution state corresponding to the operation. After each node completes its execution, the robot writes the node's execution state into the shared state management space. Specifically, each node writes its execution state into the shared state management space using the `set` method of the blackboard module. The behavior tree reads the execution state from the shared state management space to determine the execution result of the current node, thereby deciding on the next scheduling action. For example, the safety pre-check node writes its verified execution state into the shared state management space, the chassis global navigation node writes its navigation success or navigation failure state into the shared state management space, and the robotic arm local vision servo node writes its robotic arm operation success state and operation completion result into the shared state management space. The default tick cycle of the behavior tree is set to 10Hz, meaning that the behavior tree reads the execution status in the shared state management space and makes corresponding scheduling judgments in each tick cycle.

[0035] After obtaining the corresponding input data from the shared state management space, each node executes its corresponding task sequentially according to the execution order. Specifically, the safety pre-check node obtains the robot's safety status from the shared state management space and determines whether the emergency stop state has not been triggered, whether the communication status is normal, whether the electrical safety status is normal, and whether the initial posture of the robotic arm is within the safe range. The safe range is determined based on the allowable range of motion of each joint of the robotic arm. The angle of each joint should be between the upper and lower limits of the allowable range of motion of that joint, which is preset by the robotic arm's factory parameters or safety operating procedures. A normal communication status means that the communication link between the robot and the remote control system is connected and the communication delay is within the preset allowable delay range. A normal electrical safety status means that all parameters of the robot's electrical system are within the preset allowable operating range, including voltage, current, insulation resistance, and other electrical parameters, which do not exceed the safe operating limits. When all the above conditions are met, a successfully verified execution status is generated and written to the shared state management space. When any of the above conditions are not met, a failed verification execution status is generated and written to the shared state management space.

[0036] After obtaining the target device number and map data from the shared state management space, the chassis global navigation node determines the target device's position based on the target device number. Based on the map data, it uses the A* algorithm to plan a global path from the robot's current position to the coarse localization area near the target device. Then, it sends the target coordinates through Nav2's NavigateToPose Action interface. Nav2 behavior tree performs global path planning and local path tracking, outputting speed commands to control the chassis to move to the vicinity of the target. The cost map includes a static layer, an obstacle layer, and an expansion layer. Local path tracking supports the DWB local planner and a Graceful Controller based on the Park-Kuipers smooth control law. It calculates the curvature κ using polar coordinates (r, φ, δ) and automatically reduces speed when the curvature is large. During movement, it detects in real time whether the chassis has reached the coarse localization area. When the chassis reaches the coarse localization area, it writes the chassis pose and navigation success status after movement to the shared state management space. When path planning fails or the navigation status is abnormal, it writes the navigation failure status to the shared state management space.

[0037] After the lifting and coarse positioning nodes obtain the chassis pose from the shared state management space, they determine the robot's current position based on the chassis pose and determine the target lifting height based on a preset lifting height threshold. The lifting height threshold is determined based on the optimal working distance of the 3D vision sensor and the expected height range of the target, so that after the lifting mechanism is adjusted to the target lifting height, the target is within the effective detection range of the 3D vision sensor. The lifting mechanism is controlled to adjust from the current height to the target lifting height, and the encoder feedback value of the lifting mechanism is obtained in real time during the adjustment process as the lifting height, so that the target enters the effective detection range of the 3D vision sensor.

[0038] After obtaining the chassis pose and lifting height from the shared state management space, the local 3D vision recognition node controls the 3D vision sensor to acquire color and depth images containing the work target based on the chassis pose and lifting height. This 3D vision sensor uses a structured light 3D camera with a depth measurement range of 0.3-6m, a depth resolution of 640×480@30fps, and a color resolution of 1920×1080. Then, the YOLO target detection algorithm is used to identify the work target from the color image and output the 2D bounding box of the work target in the image. Combined with the depth image, the 3D coordinates of the work target in the visual coordinate system are calculated by back-projection using a pinhole camera model. The ArUco marker detector calls the solvePnP algorithm to solve for the 6DoF pose as the visual recognition pose. At the same time, the visual confidence is determined based on the detection score of the target detection algorithm. The visual recognition pose and visual confidence are written into the shared state management space.

[0039] After obtaining the visually recognized pose from the shared state management space, the coordinate transformation node first manages the transformation relationships between various coordinate systems through the TF2 transformation tree. The robot's coordinate systems include the world coordinate system W, map coordinate system M, chassis coordinate system C, lifting platform coordinate system L, robotic arm base coordinate system B, end effector coordinate system E, and vision sensor coordinate system V. The transformation relationship between these coordinate systems is represented by a 4×4 homogeneous transformation matrix T = [R t; 0 1], where R is a 3×3 rotation matrix and t is a 3×1 translation vector. The coordinate transformation chain is defined by the following relationship: in, , p_M^C is a 3×3 rotation matrix from the map coordinate system to the chassis coordinate system, and p_M^C is a 3×1 translation vector from the map coordinate system to the chassis coordinate system, which is determined by the positioning results of the chassis global navigation node. h is the height of the lifting platform, which is measured in real time by the encoder of the lifting mechanism; , Let be the 3×3 rotation matrix from the lifting platform coordinate system to the robot arm base coordinate system. It is a 3×1 translation vector from the lifting platform coordinate system to the robot arm base coordinate system, which is determined by the robot arm installation parameters and is a fixed value; The results are obtained by solving the forward kinematics of the robotic arm and calculating the DH parameters of the robotic arm.

[0040] Then, the coordinate transformation node transforms the visual pose recognition from the visual coordinate system to the robot arm's base coordinate system. Let the visual pose recognition be... Given the pose of the target object in the visual coordinate system, the transformation to the robot arm's base coordinate system is as follows: in The hand-eye calibration matrix is ​​predetermined through hand-eye calibration. Hand-eye calibration uses the easy_handeye2 calibration package. Multiple end effector-base transformation and marker-camera transformation pairs are input via OpenCV's cv2.calibrateHandEye() function, and the Tsai-Lenz method is used to solve for the hand-eye transformation matrix. Collect no fewer than 20 sets of pose data to ensure uniform distribution of motion angles among the sets. Save the calibration results in YAML format. Load the broadcast through a static TF publisher during runtime. Verify the calibration accuracy by evaluating the reprojection error. The average reprojection error is less than 0.5 pixels.

[0041] For installation operations, the grasping offset must also be superimposed on the target pose. The final target pose that the robotic arm can execute is: The coordinate unification transformation node writes the final target pose obtained from the above transformation into the target pose that the robot arm can execute, into the shared state management space. This unifies the visual recognition results into the robot arm's base coordinate system after hand-eye calibration and coordinate system transformation, providing an accurate pose reference for the robot arm to perform its tasks.

[0042] After obtaining the voltage testing result and safety status from the shared state management space, the voltage testing condition judgment node determines whether the voltage testing result is no power and the result is valid, and whether the safety status is normal. When the voltage testing result is no power and valid and the safety status is normal, a successful judgment execution status is generated and written to the shared state management space. When the voltage testing result is energized or invalid or the safety status is abnormal, a failed judgment execution status is generated and written to the shared state management space.

[0043] After the local vision servo node of the robotic arm obtains the target pose, voltage detection result and safety status of the robotic arm from the shared state management space, it calls the inverse kinematics solver to calculate the target angle of each joint of the robotic arm based on the target pose of the robotic arm. The inverse kinematics solver adopts the KDLKinematicsPlugin of the KDL library and solves the problem based on the Jacobian matrix iterative numerical solution method. It supports joint space planning and Cartesian space linear planning. After the solution is successful, the JointTrajectoryController issues joint position commands to control the robotic arm to move to the target pose of the robotic arm and controls the end effector to perform clamping, alignment, installation or removal operations.

[0044] During the gripping, alignment, installation, or removal operations performed by the end effector of a robotic arm, a force-position hybrid control strategy is employed to achieve compliant operation. Specifically, a force-position hybrid control model is established in Cartesian space, decomposing the motion of the end effector into position control direction and force control direction. Let the desired pose of the end effector be... The actual pose is Expected contact force is The actual contact force is Then the impedance control law is: Where M, B, and K are the desired inertia matrix, damping matrix, and stiffness matrix, respectively, all set as diagonal positive definite matrices. Through this impedance control law, when the end effector contacts the target, the control system dynamically adjusts the end effector's motion based on the deviation between the actual contact force and the desired contact force. This allows the robotic arm to possess force compliance capability while maintaining position control, thereby avoiding damage to the robotic arm or the target caused by rigid collisions due to pure position control. During control, a force / torque sensor collects real-time contact force feedback from the end effector. This sensor is installed between the end effector and the gripper, with a range of 300N / 30N·m, a resolution of 0.1N / 0.01N·m, and a sampling frequency of 1000Hz. The contact force threshold is dynamically set according to the operation stage: Fth=5N for the pre-alignment stage, Fth=10N for the low-speed approach stage, Fth=50N for the gripping stage, and Fth=80N for the tightening stage. When the actual contact force exceeds the contact force threshold corresponding to the current operation stage, the robotic arm stops its current action and triggers the corresponding abnormal recovery strategy. The force control loop operates at a frequency of 100Hz, while the position control loop operates at a frequency of 250Hz. Force and position coordination is achieved through periodic synchronization. For tightening operations, the end effector is controlled at a constant low speed, and collision detection is used to achieve near-force control. The grippers are controlled to open and close via digital I / O pulse signals.

[0045] Figure 2 This is a behavior tree flowchart provided for this embodiment. Figure 2 As shown, the behavior tree uses the root node as the starting point of the entire scheduling process. After receiving the task from the remote control system, the root node organizes the execution of each node sequentially according to the preset task flow through the main sequence node. The main sequence node first enters the safety pre-check node. After passing the check, it enters the chassis global navigation node. After navigation, it enters the lifting and coarse positioning node. After lifting, it enters the local 3D vision recognition node. After recognition, it enters the coordinate unified transformation node. After transformation, it enters the power testing condition judgment node. After power testing, it enters the robotic arm local vision servo node. After the robotic arm completes the task, it enters the grounding wire installation or removal node according to the task type. During the execution of the above nodes, when a node fails, the behavior tree triggers the corresponding abnormal recovery strategy by selecting a node and tries to execute alternative action nodes or recovery nodes in sequence. During the abnormal recovery process, the retry node is used to repeatedly try to execute the specified node. When all alternative actions of the selected node fail and the number of retries of the retry node reaches the preset threshold, the behavior tree controls the robot to enter the safe exit state. Once all nodes in the main sequential node have executed successfully, the behavior tree completes the entire scheduling process for this job, generates a job completion signal, and uploads the job result through the status feedback module.

[0046] S04: When the execution status is successful, the robot is controlled to execute the next node corresponding to the node according to the behavior tree and the execution order, until all nodes corresponding to this task are executed, so that the robot can complete this task. In a preferred embodiment of this invention, when the execution status is successful, the robot is controlled to execute the next node corresponding to that node according to the behavior tree and the execution order, until all nodes corresponding to the current task have been executed, so that the robot completes the current task. Specifically: In one embodiment, after the behavior tree determines that the current node has been successfully executed based on the execution status in the shared state management space, the robot controls the behavior tree to execute the next node corresponding to that node according to the execution order. Specifically, the behavior tree organizes the execution flow of each node through sequential nodes. Sequential nodes execute their child nodes in the order of execution. When the current node in the execution order is successfully executed, the sequential node automatically moves to the next child node in the execution order. Taking the installation operation as an example, after the safety pre-check node is successfully executed, the behavior tree automatically enters the chassis global navigation node; after the chassis global navigation node is successfully executed, the behavior tree automatically enters the lifting and coarse positioning node; after the lifting and coarse positioning node is successfully executed, the behavior tree automatically enters the local 3D vision recognition node; after the local 3D vision recognition node is successfully executed, the behavior tree automatically enters the coordinate unified transformation node; after the coordinate unified transformation node is successfully executed, the behavior tree automatically enters the voltage testing condition judgment node; after the voltage testing condition judgment node is successfully executed, the behavior tree automatically enters the robotic arm local vision servo node; after the robotic arm local vision servo node is successfully executed, the behavior tree automatically enters the grounding wire installation node.

[0047] The behavior tree schedules each node in the execution sequence sequentially as described above. Before execution, each node obtains its corresponding input data from the shared state management space. After execution, it writes its execution state into the shared state management space. The behavior tree reads the execution state from the shared state management space and performs scheduling checks every tick cycle (default setting is 10Hz). When the current node in the execution sequence returns a success message, the behavior tree controls the sequential node to proceed to the next node in the execution sequence, continuing execution until all nodes corresponding to this task have been completed. When all nodes in the execution sequence have executed successfully, it indicates that the robot has successfully completed the entire process of this task. The robot generates a task completion signal and writes the task result into the shared state management space. The task result is then uploaded to the remote control system via the state feedback module, providing a basis for subsequent task management and state recording.

[0048] It should be noted that during the above execution process, the sequential nodes of the behavior tree only proceed to the next node when the current node returns a successful execution. If a node returns a failed execution, the sequential node will not continue to execute subsequent nodes, but will trigger the exception recovery strategy corresponding to that node. This part will be described in detail in subsequent embodiments.

[0049] S05: When the execution status is failure, trigger the abnormal recovery strategy corresponding to the node to complete the task corresponding to the node and continue to execute the next node corresponding to the node. If the robot fails to execute the node, control the robot to exit safely.

[0050] In a preferred embodiment of this example, when the execution status is failure, an exception recovery strategy corresponding to that node is triggered to complete the task corresponding to that node and continue executing the next node corresponding to that node. If the robot fails to execute that node, the robot is controlled to safely exit. Specifically: In one embodiment, when the behavior tree determines that the current node has failed based on the execution status in the shared state management space, the robot triggers the anomaly recovery strategy corresponding to that node. Specifically, the behavior tree implements the anomaly recovery strategy through a structured combination of selection nodes and retry nodes. The selection node is used to sequentially attempt to execute the corresponding alternative action nodes or recovery nodes when the current node fails, and the retry node is used to repeatedly attempt to execute the specified node. This allows for a structured and unified implementation of recovery operations corresponding to different anomaly types at the behavior tree level, without the need to hardcode recovery logic within each node.

[0051] For different types of node execution failures, the behavior tree triggers different anomaly recovery strategies. When the chassis global navigation node fails, i.e., path planning fails or navigation status is abnormal, the behavior tree triggers the anomaly recovery strategy corresponding to the chassis global navigation node. This anomaly recovery strategy is as follows: First, the behavior tree triggers the operation of replanning the global path by selecting a node, that is, it calls the global path planner again to plan the global path from the robot's current position to the coarse positioning area near the target device based on map data; if the replanning is successful, the subsequent chassis movement operation continues; if the replanning fails, the node is selected to trigger a local obstacle avoidance operation, that is, it controls the robot's chassis to avoid obstacles; if the replanning or local obstacle avoidance fails and the number of retries reaches a preset threshold (10 retries for each of the two-layer RecoveryNode defined in the navigation behavior tree XML), the behavior tree writes the navigation failure status to the shared state management space and controls the robot to enter a safe exit state. When a local 3D vision recognition node fails to execute, i.e., when the visual confidence level is lower than the preset confidence threshold or recognition fails, the behavior tree triggers the anomaly recovery strategy corresponding to the local 3D vision recognition node. This anomaly recovery strategy is as follows: the behavior tree selects nodes and sequentially attempts to control the 3D vision sensor to re-acquire color and depth images, adjust the height of the lifting mechanism, and control the chassis for fine-tuning. After each adjustment, the local 3D vision recognition node is re-triggered to perform the recognition operation. If the re-recognition is successful, subsequent nodes continue to be executed. If all recovery attempts fail, the behavior tree controls the robot to enter a safe exit state. When a local visual servo node of the robotic arm fails to execute (i.e., the target pose of the robotic arm exceeds its workspace or the inverse kinematics solution fails), the behavior tree triggers the anomaly recovery strategy corresponding to the local visual servo node. This strategy involves the behavior tree selecting a node to first perform end-effector pose replanning, i.e., readjusting the approach posture of the end effector to attempt to find a reachable target pose under the current chassis and lifting position. If the end-effector pose replanning succeeds, the current task continues. If the end-effector pose replanning fails, the selected node further controls the robot's chassis for fine-tuning and re-executes the chassis global navigation node and the local 3D visual recognition node to obtain a new target pose. If all the above recovery attempts fail, the behavior tree controls the robot to enter a safe exit state. When the safety pre-check node or the electrical condition judgment node fails to execute, the behavior tree triggers the corresponding anomaly recovery strategy, which directly controls the robot to enter a safe exit state without attempting recovery.

[0052] During the execution of the above-mentioned anomaly recovery strategy, if recovery is successful, the behavior tree continues to execute subsequent nodes in the sequence. That is, the task corresponding to the current node is completed, and the behavior tree proceeds to the next node in the execution order. If the robot fails to execute the node, i.e., all preset recovery attempts fail and the number of retries reaches a preset threshold, the behavior tree controls the robot to enter a safe exit state and stops executing the current task. By combining node selection and retry nodes, the behavior tree executes corresponding recovery strategies for different anomaly types such as navigation failure, visual recognition failure, inverse kinematics solution failure, and safety anomalies. This allows the system to perform targeted recovery operations based on the source of the anomaly without implementing recovery logic individually within each node. This reduces the cohesion complexity of each node while improving the overall robustness of the system in dealing with various execution anomalies.

[0053] In summary, this application constructs a shared state management space independent of each node. Based on the job type, it determines the node sequence corresponding to the current job from a pre-defined node library and maps this sequence to a behavior tree. This allows each node to obtain input data from this shared space before execution and write its execution state back to the shared space after execution. Data flow between nodes is entirely completed through this shared space without relying on direct calls between nodes. When adding, replacing, or modifying any node, there is no need to adjust the internal implementation or interface of other nodes, thus severing the direct dependencies between nodes, reducing system coupling, and improving system scalability and maintainability. Simultaneously, the behavior tree makes unified scheduling decisions based on the execution state in the shared space. Upon successful execution, it schedules the next node according to the execution order; upon failure, it triggers a response related to that node. The corresponding anomaly recovery strategy, implemented in a structured manner at the behavior tree level, uniformly realizes the process connection and anomaly transition between nodes. This replaces the traditional approach of relying on hard-coded internal nodes or additional state machines to manage anomaly branches. Each node only needs to focus on its own operational logic without handling the details of process connection and anomaly recovery, reducing the cohesion complexity of each node and improving the readability of the task flow. Furthermore, when a node fails to execute, the behavior tree triggers the corresponding anomaly recovery strategy. Execution of subsequent nodes continues only after successful recovery; otherwise, the robot safely exits. This allows the system to perform targeted recovery operations based on the source of the anomaly without implementing recovery logic individually within each node. This reduces the coupling and cohesion complexity between nodes while improving the overall robustness of the system in handling various execution anomalies. This application effectively solves the problem that existing technologies cannot accurately and efficiently control robots based on behavior trees.

[0054] Example 2 Please refer to Figure 3 This is a behavior tree-based robot control device provided in the embodiments of this application.

[0055] In this embodiment, the behavior tree-based robot control device includes a first acquisition module 10, a second acquisition module 20, a control module 30, an execution module 40, and a recovery module 50.

[0056] The first acquisition module 10 is used to acquire the robot's task instructions, initialize the behavior tree according to the task instructions, and construct a shared state management space based on the task information in the task instructions. The second acquisition module 20 is used to obtain a preset robot control node library, and to determine the nodes corresponding to the current task and the execution order of the nodes from the node library according to the task type in the task instruction. The control module 30 is used to map each node to the behavior tree so that the robot can schedule the corresponding nodes in sequence according to the behavior tree and the execution order. For any node, the input data corresponding to the node is obtained from the shared state management space to control the robot to execute the task corresponding to the node and obtain the robot's execution status of the task corresponding to the node. Execution module 40 is used to control the robot to execute the next node corresponding to the current node according to the behavior tree and the execution order when the execution status is successful, until all nodes corresponding to the current task are executed, so that the robot can complete the current task. The recovery module 50 is used to trigger the abnormal recovery strategy corresponding to the node when the execution status is failure, so as to continue to execute the next node corresponding to the node after completing the task corresponding to the node. If the robot fails to execute the node, the robot is controlled to exit safely.

[0057] For ease of description and brevity, the embodiments of the device of the present invention include all the implementation methods described in the above embodiments of the robot control method based on behavior tree, and will not be repeated here.

[0058] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the scope of protection of the present invention. In particular, it should be noted that any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention for those skilled in the art.

Claims

1. A robot control method based on behavior trees, characterized in that, include: Obtain the robot's task instructions, initialize the behavior tree based on the task instructions, and construct a shared state management space based on the task information in the task instructions; Obtain a preset robot control node library, and determine the nodes corresponding to this task and the execution order of each node from the node library according to the task type in the task instruction. Each node is mapped to the behavior tree so that the robot can schedule the corresponding nodes sequentially according to the behavior tree and the execution order. For any node, the input data corresponding to the node is obtained from the shared state management space to control the robot to execute the task corresponding to the node and obtain the robot's execution state of the task corresponding to the node. When the execution status is successful, the robot is controlled to execute the next node corresponding to the node according to the behavior tree and the execution order, until all nodes corresponding to this task are executed, so that the robot can complete this task. When the execution status is failure, the abnormal recovery strategy corresponding to the node is triggered to complete the task corresponding to the node and continue to execute the next node corresponding to the node. If the robot fails to execute the node, the robot is controlled to exit safely.

2. The robot control method based on behavior tree according to claim 1, characterized in that, The step of determining the nodes corresponding to this task and the execution order of each node from the node library according to the task type in the task instruction is as follows: When the operation type is installation operation, the nodes corresponding to this operation are determined as the safety pre-inspection node, chassis global navigation node, lifting and coarse positioning node, local three-dimensional vision recognition node, coordinate unified transformation node, voltage detection condition judgment node, robotic arm local vision servo node and grounding wire installation node. When the operation type is demolition operation, the nodes corresponding to this operation are determined as the safety pre-inspection node, chassis global navigation node, lifting and coarse positioning node, local three-dimensional vision recognition node, coordinate unified transformation node, voltage detection condition judgment node, robotic arm local vision servo node and demolition node. For any of the job types, the execution order is the sequential order of the nodes mentioned above within that job type.

3. The robot control method based on behavior tree according to claim 2, characterized in that, The step of obtaining the input data corresponding to the node from the shared state management space to control the robot to execute the task corresponding to the node specifically includes: The shared status management space stores the job type, target equipment number, map data, chassis pose, lifting height, visual recognition pose, robotic arm target pose, visual confidence level, voltage detection result, safety status, and job result. When the node is a safety pre-check node, the robot is controlled to obtain the safety status from the shared state management space, and the safety status is verified to obtain the verified execution status. Write the verified execution status into the shared status management space; When the node is a chassis global navigation node, the robot is controlled to obtain the target device number and the map data from the shared state management space, and the target device position is determined according to the target device number. The robot's chassis is then controlled to move to the coarse positioning area near the target device to obtain the chassis pose and navigation state after the movement. The chassis pose and navigation state after the movement are then written into the shared state management space. When the node is a lifting and coarse positioning node, the robot is controlled to obtain the chassis pose from the shared state management space, so as to control the robot's built-in lifting mechanism to adjust the height to enter the detection range of the robot's built-in vision sensor, obtain the adjusted lifting height, and write the adjusted lifting height into the shared state management space. When the node is a local 3D visual recognition node, the robot is controlled to obtain the chassis pose and lifting height from the shared state management space to identify the task target of this operation and output the visual recognition pose and visual confidence score, and write the visual recognition pose and visual confidence score into the shared state management space. When the node is a coordinate unified transformation node, the robot is controlled to obtain the visual recognition pose from the shared state management space, convert the visual recognition pose into a target pose that the robot's robotic arm can execute, and write the target pose into the shared state management space. When the node is a voltage testing condition judgment node, the robot is controlled to obtain the voltage testing result from the shared state management space, and the voltage testing result is judged to obtain the judgment execution state; the judgment execution state is written into the shared state management space. When the node is a local vision servo node of the robotic arm, the robot is controlled to obtain the target pose of the robotic arm, the electrical detection result and the safety status from the shared state management space, so as to control the robotic arm to perform the corresponding operation and obtain the robotic arm status and operation result; the robotic arm status and operation result are written into the shared state management space.

4. The robot control method based on behavior tree according to claim 3, characterized in that, The scheduling process for the job corresponding to the safety pre-check node is as follows: When the node is a safety pre-check node, the robot is controlled to obtain the safety status from the shared state management space; the safety status includes emergency stop status, communication status, electrical safety status, and robot arm initial posture; When the emergency stop state is not triggered, the communication state is within a preset first normal range, the electrical safety state is within a preset second normal range, and the robot arm's initial posture is within a preset safety range, a successfully verified execution state is generated and written into the shared state management space. Otherwise, an execution status indicating a verification failure is generated and written to the shared status management space, and the exception recovery strategy corresponding to the security pre-check node is triggered. The anomaly recovery strategy is to control the robot to enter a safe exit state.

5. The robot control method based on behavior tree according to claim 3, characterized in that, The scheduling process for the job corresponding to the chassis global navigation node is as follows: When the node is a chassis global navigation node, the robot is controlled to obtain the target device number and the map data from the shared state management space; The location of the target device is determined based on the target device number, and a global path from the robot's current location to the coarse positioning area is planned based on the map data; The robot's chassis is controlled to move along the global path, and the chassis is monitored in real time to see if it reaches the coarse positioning area during the movement. When the chassis reaches the coarse positioning area, the chassis pose after movement and the navigation success status are obtained, and the chassis pose after movement and the navigation success status are written into the shared state management space. When the chassis fails to reach the coarse positioning area, the navigation status is determined to be abnormal. When the global path planning fails or the navigation status is abnormal, the navigation failure status is written to the shared status management space, triggering the abnormal recovery strategy corresponding to the global navigation node of the chassis. The abnormal recovery strategy is to replan the global path or control the chassis to perform local obstacle avoidance.

6. The robot control method based on behavior tree according to claim 3, characterized in that, The scheduling process for the jobs corresponding to the lifting and coarse positioning nodes is as follows: When the node is a lifting and coarse positioning node, the robot is controlled to obtain the chassis pose from the shared state management space; The robot's current position is determined based on the chassis pose, and the target lifting height is determined based on a preset lifting height threshold. The robot's built-in lifting mechanism is controlled to adjust from its current position to the target lifting height, and the encoder feedback value of the lifting mechanism is obtained in real time as the lifting height during the adjustment process; When the lifting height reaches the target lifting height, the adjusted lifting height is obtained and written into the shared state management space; When the lifting height cannot reach the target lifting height or the encoder feedback is abnormal, the abnormal recovery strategy corresponding to the lifting and coarse positioning node is triggered. The abnormal recovery strategy is to re-execute the lifting adjustment or control the robot chassis to make fine adjustments. If the readjustment or fine adjustment fails, the robot is controlled to enter the safe exit state.

7. The robot control method based on behavior tree according to claim 3, characterized in that, The scheduling process for the job corresponding to the local 3D visual recognition node is as follows: When the node is a local 3D vision recognition node, the robot is controlled to obtain the chassis pose and the lifting height from the shared state management space; Based on the chassis pose and the lifting height, the robot's built-in three-dimensional vision sensor collects color and depth images of the target object for this operation. The target of the current task is identified from the color image using a target detection algorithm, and the two-dimensional bounding box of the target in the image is output. Based on the two-dimensional bounding box and the depth image, the three-dimensional coordinates of the target in the visual coordinate system are calculated as the visual recognition pose. The visual confidence level is determined according to the target detection algorithm, and the visual recognition pose and the visual confidence level are written into the shared state management space; When the visual confidence level is lower than the preset confidence threshold or the task target for this task fails to be identified, the anomaly recovery strategy corresponding to the local 3D visual recognition node is triggered.

8. The robot control method based on behavior tree according to claim 3, characterized in that, The scheduling process for the job corresponding to the coordinate transformation node is as follows: When the node is a coordinate unified transformation node, the robot is controlled to obtain the visual recognition pose from the shared state management space. The visual recognition pose is the pose of the task target in the visual coordinate system for this task. Based on preset hand-eye calibration parameters, the pose in the visual coordinate system is converted to the pose in the end-effector coordinate system of the robotic arm; Based on the transformation relationship between the robot's various coordinate systems, the pose in the end-effector coordinate system of the robotic arm is transformed to the pose in the base coordinate system of the robotic arm to obtain the target pose that the robotic arm can execute. Write the target pose into the shared state management space; When the visual recognition pose is invalid, the hand-eye calibration parameters fail to load, or the pose transformation from the end-effector coordinate system to the base coordinate system fails, an anomaly recovery strategy corresponding to the coordinate transformation node is triggered. The anomaly recovery strategy involves re-acquiring the visual recognition pose from the shared state management space and performing the coordinate transformation again. If the coordinate transformation fails again, the local 3D visual recognition node is triggered to re-perform visual recognition. If the transformation still fails after re-performing visual recognition, the robot is controlled to enter a safe exit state.

9. The robot control method based on behavior tree according to claim 3, characterized in that, The scheduling process for the task corresponding to the local vision servo node of the robotic arm is as follows: When the node is a local vision servo node of the robotic arm, the robot is controlled to obtain the target pose of the robotic arm, the electrical detection result, and the safety status from the shared state management space; The target angles of each joint of the robotic arm are calculated by calling a preset inverse kinematics solver based on the target pose of the robotic arm. The robot's robotic arm is controlled to move to the target pose of the robotic arm according to the target angle of each joint, and the end effector of the robotic arm is controlled to perform clamping, alignment, installation or removal operations. The contact force feedback of the end effector is monitored during the execution of the robotic arm's end effector; When the robotic arm reaches the target pose and the clamping, alignment, installation or dismantling operation is completed, the robotic arm operation success status and operation completion result are obtained, and the robotic arm operation success status and operation completion result are written into the shared status management space. When the target pose of the robotic arm exceeds the workspace of the robotic arm or the calculation of the target angle of each joint of the robotic arm fails, an anomaly recovery strategy corresponding to the local visual servo node of the robotic arm is triggered. The anomaly recovery strategy is to first execute a preset end-effector posture replanning scheme. If the execution of the end-effector posture replanning scheme fails, the robot's chassis is controlled to make fine adjustments and the chassis global navigation node and the local three-dimensional visual recognition node are re-executed.

10. A robot control device based on behavior trees, characterized in that, include: The first acquisition module is used to acquire the robot's task instructions, initialize the behavior tree according to the task instructions, and construct a shared state management space based on the task information in the task instructions. The second acquisition module is used to obtain a preset robot control node library. Based on the job type in the job task instruction, it determines the nodes corresponding to the current job and the execution order of the nodes from the node library. The control module is used to map each node to the behavior tree so that the robot can schedule the corresponding nodes in sequence according to the behavior tree and the execution order. For any node, the module obtains the input data corresponding to the node from the shared state management space to control the robot to execute the task corresponding to the node and obtain the robot's execution status of the task corresponding to the node. The execution module is used to control the robot to execute the next node corresponding to the current node according to the behavior tree and the execution order when the execution status is successful, until all nodes corresponding to the current task are executed, so that the robot can complete the current task. The recovery module is used to trigger the abnormal recovery strategy corresponding to the node when the execution status is failure, so as to continue to execute the next node corresponding to the node after completing the task corresponding to the node. If the robot fails to execute the node, the robot is controlled to exit safely.

Images