Method and device for planning a robot arm path for a nuclear power plant

Abstract

The application relates to a nuclear power robot mechanical arm path planning method and device. The method comprises the following steps: obtaining work environment data of a nuclear power robot, and constructing a collision model; determining a respective end reachable area of a main arm and an auxiliary arm and a cooperative reachable area of the main arm and the auxiliary arm; determining a target parking pose of a mobile chassis of the nuclear power robot and a respective target joint configuration of the main arm and the auxiliary arm; in the case that the mobile chassis moves to the target parking pose, one of the auxiliary arm and the main arm is sequentially taken as an obstacle, the collision model is updated, and a motion path of the other of the auxiliary arm and the main arm from a current joint configuration to a corresponding target joint configuration is planned according to the updated collision model; in the case that the main arm and the auxiliary arm move to the respective target joint configurations and clamp a target object, a synchronous joint motion path of the main arm and the auxiliary arm is planned according to a target motion track of the target object. The method can improve the work efficiency and the autonomous level of nuclear emergency disposal.

Description

Technical Field

[0001] This application relates to the field of nuclear power technology, and in particular to a method and apparatus for path planning of a robotic arm for a nuclear power robot. Background Technology

[0002] Nuclear accident emergency environments are highly uncertain and extremely dangerous, characterized by high radiation doses, severe structural damage, widespread debris accumulation, confined spaces, and complex terrain, making it virtually impossible for personnel to enter and perform cleanup and operational tasks. Therefore, specialized robots with strong mobility, high payload capacity, and high autonomy have become key equipment for nuclear emergency response.

[0003] Currently, robots used in nuclear emergency operations are mainly divided into two categories. One category is small detection robots used in nuclear radiation environments. These robots employ a tracked chassis and a single robotic arm structure, enabling them to traverse complex terrains such as rubble and collapsed surfaces. However, due to their single-arm structure, they lack steady-state dual-point gripping capabilities, making it difficult to perform stable grasping and handling tasks for large objects. The other category is industrial robots with dual-arm structures. While they can achieve coordinated operation of both arms, they typically rely on fixed bases or structured working environments. They lack mobile platforms adapted to unstructured and complex terrains after a nuclear disaster, making it impossible for them to move flexibly and operate autonomously in nuclear emergency sites with accumulated debris and confined spaces.

[0004] Therefore, there is an urgent need for a path planning method for the robotic arm of a nuclear power robot that can improve the operational efficiency and autonomy of nuclear emergency response. Summary of the Invention

[0005] Therefore, it is necessary to provide a method and device for planning the path of a nuclear power robot arm that can improve the operational efficiency and autonomy of nuclear emergency response, addressing the aforementioned technical problems.

[0006] In a first aspect, this application provides a method for path planning of a robotic arm for a nuclear power plant robot, including:

[0007] Acquire the operating environment data of the nuclear power robot, and construct a collision model based on the operating environment data. The collision model includes the geometric feature data of obstacles in the operating environment and the boundary of the operating environment.

[0008] Based on the joint configuration data of the main arm and the auxiliary arm of the nuclear power robot, the cooperative reachable area of ​​the main arm and the auxiliary arm is determined;

[0009] Based on the location information of the target object and the cooperative reachable area, the target docking posture of the mobile chassis of the nuclear power robot and the target joint configuration of the main arm and the auxiliary arm are determined.

[0010] When the mobile chassis moves to the target docking position, one of the secondary arm and the main arm is sequentially designated as an obstacle, the collision model is updated, and the motion path of the other of the secondary arm and the main arm from the current joint configuration to the corresponding target joint configuration is planned according to the updated collision model. The motion path is determined by fusing random expansion direction and potential field guidance direction. The random expansion direction is the direction from the node in the random expansion tree to the random sampling point, and the potential field guidance direction is the opposite direction of the total potential field gradient direction. The total potential field is determined according to the updated collision model and the target joint configuration.

[0011] When the main arm and the auxiliary arm move to their respective target joint configurations and clamp the target object, the synchronous joint movement paths of the main arm and the auxiliary arm are planned according to the target movement trajectory of the target object.

[0012] In one embodiment, the step of sequentially treating one of the secondary arm and the primary arm as an obstacle, updating the collision model, and planning the motion path of the other of the secondary arm and the primary arm from the current joint configuration to the corresponding target joint configuration based on the updated collision model includes:

[0013] When the mobile chassis moves to the target docking position, the auxiliary arm is added as an obstacle to the collision model according to the current joint configuration of the auxiliary arm, so as to update the collision model. Based on the updated collision model, the motion path of the main arm from the current joint configuration to the corresponding target joint configuration is planned.

[0014] When the main arm moves to the corresponding target joint configuration, the main arm is added as an obstacle to the collision model according to the target joint configuration of the main arm to update the collision model, and the motion path of the secondary arm from the current joint configuration to the corresponding target joint configuration is planned according to the updated collision model.

[0015] In one embodiment, the motion path planning method includes:

[0016] Obtain the initial joint configuration and the preset expansion step size, and construct a random expansion tree with the initial joint configuration as the root node;

[0017] Random sampling points are generated in the joint configuration space, and the node closest to the random sampling point is found in the current random expansion tree as the nearest node. The joint configuration space is determined by the angle range of each joint where the target joint configuration and the initial joint configuration are located. The points in the joint configuration space are used to represent a set of joint variable values.

[0018] Using the target joint as the attraction source and the obstacle as the repulsion source, an attraction potential field and a repulsion potential field are constructed, superimposed to obtain a total potential field, and the gradient direction of the total potential field at the nearest node is calculated.

[0019] The direction from the nearest node to the random sampling point is taken as the random expansion direction, and the opposite direction of the total potential gradient direction is taken as the potential field guiding direction. The random expansion direction and the potential field guiding direction are weighted and fused by a preset fusion weight to determine the target expansion direction. New nodes are generated along the target expansion direction according to the expansion step size.

[0020] Collision detection is performed on the joint configuration corresponding to the new node according to the updated collision model. If the collision detection result is that no collision has occurred, the new node is added to the random expansion tree.

[0021] If the distance between the new node and the target joint configuration is less than or equal to a preset threshold, the motion path from the initial joint configuration to the target joint configuration is output.

[0022] In one embodiment, constructing a collision model based on the operating environment data includes:

[0023] Based on the described work environment data, a three-dimensional environment model is constructed;

[0024] Extract non-convex obstacle data and environment boundary data from the three-dimensional environment model;

[0025] The non-convex obstacle data and the environmental boundary data are subjected to convex decomposition processing to obtain a combination of multiple approximate convex geometries corresponding to each.

[0026] A collision model is constructed based on the combination of the multiple approximate convex geometries.

[0027] In one embodiment, the step of planning the synchronous joint motion path of the main arm and the auxiliary arm based on the target motion trajectory of the target object includes:

[0028] Determine the first rigid body transformation relationship between the target object center coordinate system and the first gripping point coordinate system, and the second rigid body transformation relationship between the target object center coordinate system and the second gripping point coordinate system;

[0029] Based on the pose sequence of the target object's center coordinate system along the target's motion trajectory, the first rigid body transformation relationship, and the second rigid body transformation relationship, determine the pose sequences of the first grasping point and the second grasping point respectively;

[0030] Based on the relative pose relationship between the first gripping point and the end effector of the main arm, the end effector pose sequence of the main arm is determined, and based on the relative pose relationship between the second gripping point and the end effector of the secondary arm, the end effector pose sequence of the secondary arm is determined.

[0031] Based on the end-effector pose sequences of the main arm and the auxiliary arm, the synchronous joint motion paths of the main arm and the auxiliary arm are planned.

[0032] In one embodiment, the step of planning the synchronous joint motion path of the main arm and the auxiliary arm based on the respective end-effector pose sequences of the main arm and the auxiliary arm includes:

[0033] Inverse kinematics is performed on the end-effector pose sequence of the main arm to generate the joint motion path of the main arm;

[0034] Inverse kinematics is performed on the end-effector pose sequence to generate the joint motion path of the arm.

[0035] Based on the time synchronization relationship between the joint motion paths of the main arm, the joint motion paths of the auxiliary arm, and the end-effector pose sequence, the synchronized joint motion paths of the main arm and the auxiliary arm are planned.

[0036] In one embodiment, determining the cooperative reachable area of ​​the main arm and the auxiliary arm based on their respective joint configuration data includes:

[0037] Establish link coordinate systems for the main arm and auxiliary arm of the nuclear power robot respectively and determine the corresponding kinematic parameters;

[0038] Based on the link coordinate system and the kinematic parameters, the forward kinematic model of the main arm and the forward kinematic model of the auxiliary arm are constructed respectively.

[0039] Discrete sampling is performed on each joint variable of the main arm and the auxiliary arm within each joint angle range to obtain the sampled value of each joint variable;

[0040] By traversing the joint angle combinations within the range of each joint angle, the sampled values ​​of each joint of the main arm are substituted into the forward kinematics model of the main arm to obtain the set of reachable points of the main arm, which is associated with the end reachable region of the main arm.

[0041] By traversing the joint angle combinations within the range of each joint angle, the sampled values ​​of each joint of the auxiliary arm are substituted into the positive kinematics model of the auxiliary arm to obtain the set of reachable points of the auxiliary arm, which is associated with the end reachable region of the auxiliary arm.

[0042] Perform an intersection operation on the set of reachable points of the main arm and the set of reachable points of the secondary arm, and determine the cooperative reachable region based on the result of the intersection operation.

[0043] In one embodiment, constructing the forward kinematics model of the main arm and the forward kinematics model of the secondary arm respectively includes:

[0044] Determine the transformation relationship between adjacent joint coordinate systems in the main arm and the auxiliary arm respectively;

[0045] The transformation relationships between the coordinate systems of each adjacent joint corresponding to the main arm and the auxiliary arm are combined sequentially to obtain the overall transformation relationship from the base coordinate system to the end effector coordinate system;

[0046] The overall transformation relationship corresponding to the main arm is taken as the positive kinematic model of the main arm;

[0047] The overall transformation relationship corresponding to the secondary arm is taken as the forward kinematic model of the secondary arm.

[0048] In one embodiment, the method for determining the target docking pose includes:

[0049] Based on the location information of the target object and the cooperative reachable area, a set of dockable poses for the mobile chassis is determined, wherein the dockable poses in the set of dockable poses are poses that allow the target object to be located within the cooperative reachable area.

[0050] According to preset optimization rules, the target parking position of the mobile chassis is selected from the set of parking positions.

[0051] Secondly, this application also provides a path planning device for the robotic arm of a nuclear power robot, comprising:

[0052] The acquisition module is used to acquire the working environment data of the nuclear power robot and construct a collision model based on the working environment data. The collision model includes the geometric feature data of obstacles in the working environment and the boundary of the working environment.

[0053] The reachable area determination module is used to determine the cooperative reachable area of ​​the main arm and the auxiliary arm based on the joint configuration data of the main arm and the auxiliary arm of the nuclear power robot.

[0054] The motion parameter determination module is used to determine the target docking posture of the mobile chassis of the nuclear power robot and the target joint configuration of the main arm and the auxiliary arm, based on the position information of the target object and the cooperative reachable area.

[0055] The path planning module is used to, when the mobile chassis moves to the target docking pose, sequentially treat one of the secondary arm and the main arm as an obstacle, update the collision model, and plan the motion path of the other of the secondary arm and the main arm from the current joint configuration to the corresponding target joint configuration based on the updated collision model; wherein, the motion path is determined by fusing random expansion direction and potential field guidance direction, the random expansion direction is the direction from the node in the random expansion tree to the random sampling point, the potential field guidance direction is the opposite direction of the total potential field gradient direction, and the total potential field is determined based on the updated collision model and the target joint configuration;

[0056] The path planning module is further configured to plan the synchronous joint motion path of the main arm and the auxiliary arm according to the target motion trajectory of the target object when the main arm and the auxiliary arm move to their respective target joint configurations and clamp the target object.

[0057] Thirdly, this application also provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the method described in any of the above claims.

[0058] Fourthly, this application also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method described in any of the preceding claims.

[0059] Fifthly, this application also provides a computer program product, including a computer program that, when executed by a processor, implements the steps of the method described in any of the above claims.

[0060] The aforementioned robotic arm path planning method and device for nuclear power robots, through environmental modeling, accessibility analysis, chassis docking and dual-arm configuration linkage planning, and the use of a path search method that combines random expansion and potential field guidance during path planning, achieves safe, efficient and autonomous path generation for the entire process of robot handling operations in nuclear emergency scenarios. It is suitable for extreme operating environments in nuclear accident emergency scenarios such as strong radiation, structural collapse, narrow space, complex terrain and many obstacles, reducing the need for remote human intervention in strong radiation environments and improving the operational efficiency and autonomy of nuclear emergency response. Attached Figure Description

[0061] To more clearly illustrate the technical solutions in the embodiments of this application or related technologies, the drawings used in the description of the embodiments of this application or related technologies will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0062] Figure 1 This is a flowchart illustrating the path planning method for the robotic arm of a nuclear power robot in one embodiment.

[0063] Figure 2 This is a flowchart illustrating the steps in one embodiment of sequentially treating one of the secondary arm and the main arm as an obstacle, updating the collision model, and planning the motion path of the other of the secondary arm and the main arm from the current joint configuration to the corresponding target joint configuration based on the updated collision model.

[0064] Figure 3 This is a flowchart illustrating the motion path planning method in one embodiment;

[0065] Figure 4 This is a flowchart illustrating the single-arm RRT-APF path planning algorithm in one embodiment;

[0066] Figure 5 This is a schematic diagram illustrating the process of constructing an environmental boundary collision model in one embodiment;

[0067] Figure 6 This is a schematic diagram of a multi-stage hybrid path planning framework in one embodiment;

[0068] Figure 7 This is a structural block diagram of the robotic arm path planning device for a nuclear power robot in one embodiment. Detailed Implementation

[0069] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0070] It should be noted that the terms "first," "second," etc., used in this application can be used to describe various elements, but these elements are not limited by these terms. These terms are only used to distinguish the first element from the second element. The terms "comprising" and "having," and any variations thereof, used in this application, are intended to cover non-exclusive inclusion. The term "multiple" used in this application refers to two or more. The term "and / or" used in this application refers to one of the embodiments, or any combination of multiple embodiments.

[0071] In one embodiment, such as Figure 1 As shown, a path planning method for the robotic arm of a nuclear power plant robot is provided. This embodiment illustrates the application of this method to a terminal. It is understood that this method can also be applied to a server, and further to a system including both a terminal and a server, and implemented through interaction between the terminal and the server. In this embodiment, the method includes the following steps:

[0072] Step 110: Obtain the operating environment data of the nuclear power robot, and construct a collision model based on the operating environment data. The collision model includes the geometric feature data of obstacles in the operating environment and the boundary of the operating environment.

[0073] Nuclear power robot is a type of nuclear power robot that features a mobile chassis, a main arm, and a secondary arm. Geometric feature data refers to data that represents the shape, size, position, and orientation of obstacles or environmental boundaries. In one possible implementation, the sensors and controllers of the nuclear power robot are designed to be radiation-resistant to adapt to the nuclear radiation environment.

[0074] For example, a nuclear power robot may include a tracked mobile chassis, a rotating platform mounted on the mobile chassis, and a main arm and a secondary arm mounted on the rotating platform. The nuclear power robot can acquire 3D point cloud data of the working environment using multi-line LiDAR and image data of the working environment using a camera device mounted on the robot platform or the end effector of the robotic arm. A raster map, a 3D voxel map, or a 3D mesh model can be constructed based on the 3D point cloud data, and the boundaries of target objects and obstacles can be supplemented and determined based on the image data to obtain working environment data. Non-convex obstacles and environmental boundaries in the working environment can be converted into a set of geometric feature data that can be used by convex collision detection algorithms to obtain a collision model.

[0075] Step 120: Determine the cooperative reachable area of ​​the main arm and the auxiliary arm based on the joint configuration data of the main arm and the auxiliary arm of the nuclear power robot.

[0076] Joint configuration data represents the kinematic constraints of the main arm or auxiliary arm. The cooperative reachable region refers to the spatial area that both the main arm and auxiliary arm can reach and perform corresponding operations on the target object. The cooperative reachable region can be determined based on the spatial overlap between the end-effector reachable regions of the main arm and auxiliary arm. Alternatively, it can be determined based on the individual end-effector reachable regions of the main arm and auxiliary arm. The end-effector reachable region refers to the spatial area that the end effector of the robotic arm can reach, provided that the robotic arm satisfies its own joint variable ranges and mechanical structural constraints. The end-effector reachable region can represent only the position range of the end effector, or it can represent both the position and orientation range of the end effector.

[0077] For example, kinematic models of the main arm and the auxiliary arm can be established separately. The kinematic model is used to represent the mapping relationship between the joint configuration of each robotic arm and the pose of the corresponding end effector. Furthermore, multiple sets of candidate joint configurations can be generated, and each candidate joint configuration can be input into the corresponding kinematic model to determine the corresponding end effector pose; based on the multiple sets of end effector poses, the respective end-effector reachable regions of the main arm and the auxiliary arm, as well as the cooperative reachable regions of the main arm and the auxiliary arm, can be determined.

[0078] Step 130: Based on the location information of the target object and the cooperative reachable area, determine the target docking pose of the mobile chassis of the nuclear power robot and the target joint configuration of the main arm and the auxiliary arm.

[0079] For example, the position and orientation of the mobile chassis relative to the target object can be changed based on the target object's position information and the cooperative reachable area. It can then be determined whether the target object or the two gripping positions on the target object are located within the cooperative reachable area under each candidate chassis pose. Candidate chassis poses that enable the target object or the two gripping positions to meet the dual-arm cooperative reachability condition are grouped into a set of dockable poses, and the target docking pose is determined from this set. Further, based on the target object's pose information and the gripping poses corresponding to the main arm and the auxiliary arm respectively on the target object, the target end effector poses that the main arm's end effector and the auxiliary arm's end effector need to reach under the target docking pose can be determined.

[0080] Step 140: With the mobile chassis moving to the target parking pose, one of the secondary arm and the main arm is used as an obstacle in sequence, the collision model is updated, and the motion path of the other of the secondary arm and the main arm is planned from the current joint configuration to the corresponding target joint configuration based on the updated collision model. The motion path is determined by a combination of random expansion direction and potential field guidance direction. The random expansion direction is the direction from the node in the random expansion tree to the random sampling point, and the potential field guidance direction is the opposite direction of the gradient direction of the total potential field. The total potential field is determined based on the updated collision model and the target joint configuration.

[0081] For example, when the mobile chassis moves to the target parking position, the main arm can be added as an obstacle to the collision model based on the current joint configuration of the main arm, and the motion path of the secondary arm from the current joint configuration to the corresponding target joint configuration can be planned based on the updated collision model; when the secondary arm moves to the corresponding target joint configuration along the corresponding motion path, the secondary arm can be added as an obstacle to the collision model based on the target joint configuration of the secondary arm, and the motion path of the main arm from the current joint configuration to the corresponding target joint configuration can be planned based on the updated collision model.

[0082] The motion path can be determined by combining the random expansion direction with the potential field guidance direction.

[0083] Step 150: When the main arm and the auxiliary arm move to their respective target joint configurations and clamp the target object, plan the synchronous joint motion path of the main arm and the auxiliary arm according to the target motion trajectory of the target object.

[0084] The target motion trajectory can be the position trajectory of the center point of the target object, or the object center pose trajectory that includes the center position and the orientation of the target object.

[0085] For example, the target motion trajectory of the target object's center can be determined based on the handling task. The target motion trajectory may include the target object's lifting trajectory, translation trajectory, rotation trajectory, or a trajectory formed by a combination of at least two of these trajectories. Furthermore, based on the target motion trajectory of the target object, the corresponding end effector trajectories of the main arm and the auxiliary arm can be determined, and based on the corresponding end effector trajectories of the main arm and the auxiliary arm, the synchronous joint motion paths of the main arm and the auxiliary arm can be determined.

[0086] The aforementioned path planning method for the robotic arm of the nuclear power robot achieves safe, efficient, and autonomous path generation for the entire process of robot handling operations in nuclear emergency scenarios through the linkage planning of environmental modeling, accessibility analysis, chassis docking, and dual-arm configuration. Furthermore, the path planning adopts a path search method that integrates random expansion and potential field guidance. This method is suitable for extreme operating environments in nuclear accident emergency scenarios, such as strong radiation, structural collapse, narrow spaces, complex terrain, and numerous obstacles. It reduces the need for remote human intervention in strong radiation environments and improves the operational efficiency and autonomy of nuclear emergency response.

[0087] In one exemplary embodiment, such as Figure 2 As shown, the steps of sequentially treating one of the secondary arm and the main arm as an obstacle, updating the collision model, and planning the motion path of the other of the secondary arm and the main arm from the current joint configuration to the corresponding target joint configuration based on the updated collision model include:

[0088] Step A1: When the mobile chassis moves to the target parking position, the auxiliary arm is added as an obstacle to the collision model according to the current joint configuration of the auxiliary arm to update the collision model. Based on the updated collision model, the motion path of the main arm from the current joint configuration to the corresponding target joint configuration is planned.

[0089] For example, the position and orientation of each link of the secondary arm in the preset coordinate system can be determined according to the current joint configuration of the secondary arm, and the geometric model of the secondary arm under the current joint configuration can be obtained accordingly; the geometric model of the secondary arm under the current joint configuration is added to the collision model as a static obstacle to obtain the first updated collision model; according to the first updated collision model, the process of the main arm moving from the current joint configuration to the corresponding target joint configuration is subject to collision constraints, and the motion path of the main arm is planned.

[0090] Step A2: When the main arm moves to the corresponding target joint configuration, add the main arm as an obstacle to the collision model according to the target joint configuration of the main arm to update the collision model, and plan the motion path of the secondary arm from the current joint configuration to the corresponding target joint configuration according to the updated collision model.

[0091] For example, the position and orientation of each link of the main arm in the preset coordinate system can be determined according to the target joint configuration of the main arm, and the geometric model of the main arm under the target joint configuration can be obtained accordingly; the geometric model of the main arm under the target joint configuration is added to the collision model as a static obstacle to obtain a second updated collision model; according to the second updated collision model, the process of the auxiliary arm moving from the current joint configuration to the corresponding target joint configuration is subject to collision constraints, and the motion path of the auxiliary arm is planned.

[0092] In this embodiment, by planning the master and slave arms sequentially, mutual interference during the movement of the two arms can be avoided, ensuring the safety of each arm's movement and improving the feasibility and efficiency of path planning for the two arms in a narrow working space.

[0093] In one exemplary embodiment, such as Figure 3 As shown, the methods for planning motion paths include:

[0094] Step B1: Obtain the initial joint configuration and the preset expansion step size, and construct a random expansion tree with the initial joint configuration as the root node.

[0095] For example, pre-set initial joint configurations and path search parameters can be obtained. Path search parameters include, but are not limited to, the maximum number of iterations and the step size.

[0096] Step B2: Generate random sampling points in the joint configuration space, and find the node closest to the random sampling point in the current random expansion tree as the nearest node. The joint configuration space is determined by the angle range of each joint in the target joint configuration and the initial joint configuration. The points in the joint configuration space are used to represent a set of joint variable values.

[0097] For example, the joint configuration space can be determined by the allowable value range of each joint variable of the robotic arm to be planned, where each point in the joint configuration space represents a set of joint variable values ​​for the robotic arm to be planned. The robotic arm to be planned can be a main arm or a secondary arm.

[0098] Step B3: Using the target joint configuration as the attraction source and the obstacle as the repulsion source, construct the attraction potential field and the repulsion potential field, superimpose them to obtain the total potential field, and calculate the gradient direction of the total potential field at the nearest node.

[0099] For example, the target joint can be configured As an attraction source, based on the current joint configuration q and the target joint configuration... Euclidean distance between Constructing an attractive potential field The attractive potential field can be represented by a piecewise potential function:

[0100] (1)

[0101] in, For the attraction potential coefficient, The threshold for switching the attraction potential. When Not greater than When, a quadratic potential function is used; when Greater than When using a linear transition, a linear transition form is adopted.

[0102] The obstacles represented by the updated collision model can be used as repulsion sources, based on the minimum distance between the robot arm to be planned and the nearest obstacle under the joint configuration q. Constructing a repulsive potential field A repulsive potential field can be represented as:

[0103] (2)

[0104] in, The repulsive potential coefficient, This is the minimum distance threshold. When... Not greater than At that time, according to Construct a repulsive potential; when Greater than At that time, the repulsive potential is zero. The attractive potential field will be... With repulsive potential field Superposition yields the total potential field. And calculate the total potential field at the nearest node. The gradient at that point.

[0105] Step B4: Take the direction from the nearest node to the random sampling point as the random expansion direction, and take the opposite direction of the total potential gradient direction as the potential field guiding direction. Use preset fusion weights to perform weighted fusion of the random expansion direction and the potential field guiding direction to determine the target expansion direction. Generate new nodes along the target expansion direction according to the expansion step size.

[0106] For example, the nearest node can be Point to random sampling point The unit direction is determined as a random expansion direction, and the total potential field is located at the nearest node. The negative gradient unit direction at a given point is determined as the potential field guiding direction. Based on the fusion weights... The random expansion direction and the potential field guidance direction are fused, whereby... Used to control the proportion of random expansion directions in the fusion result. Used to control the proportion of the potential field guiding direction in the fusion result. Fusion weights When the value is close to 1, the fused direction is more biased towards the random expansion direction, the algorithm has stronger randomness, and the global exploration ability is enhanced. When the value is close to 0, the fused direction is more biased towards the potential field's guidance direction, and the algorithm has a stronger certainty in approaching the target. This can be achieved by adjusting... The value of can balance global exploration and local guidance in path search. New nodes are generated based on the fused target expansion direction and expansion step size. The new node can be represented as:

[0107] (3)

[0108] in, This indicates that the fused direction vector is normalized. These are preset parameters used to avoid a denominator of zero. To extend the step size, This indicates the distance the node has expanded this time.

[0109] Step B5: Perform collision detection on the joint configuration corresponding to the new node according to the updated collision model. If the collision detection result is that no collision has occurred, add the new node to the random expansion tree.

[0110] For example, it can be based on the new node A set of corresponding joint variable values ​​determines the geometric configuration of the robot arm to be planned under that joint configuration. The updated collision model is then used to detect whether the robot arm collides with environmental obstacles, the boundary of the work environment, or another robot arm acting as a static obstacle. Collision detection can employ the GJK algorithm. The GJK algorithm is an iterative algorithm used to calculate the distance between two convex polyhedra or determine whether they intersect. It utilizes support functions in the Minkowski difference space, constructing an evolving simplex to find the point closest to the origin to determine whether two convex polyhedra collide. When no collision is detected, the GJK algorithm can also output the minimum physical distance between the robot arm to be planned and the obstacle, used to assess the safety margin between the current joint configuration and the obstacle. If no collision occurs, the new node... Insert into the randomly expanded tree T, and set the nearest node Set as a new node The parent node; if a collision occurs, the new node is discarded. And regenerate random sampling points in the joint configuration space.

[0111] Optionally, if the collision detection of the joint configuration corresponding to the new node is set to cause a collision, the new node can be discarded.

[0112] Step B6: If the distance between the new node and the target joint configuration is less than or equal to a preset threshold, output the motion path from the initial joint configuration to the target joint configuration.

[0113] For example, a new node can be computed. Configuration with target joint The distance between them is determined, and it is determined whether the distance is not greater than a preset threshold. If it is not greater than the preset threshold Then it is determined that the initial joint configuration has been found. To the target joint configuration The system calculates feasible collision avoidance paths and determines and outputs the movement path based on the random expansion tree T. If the current iteration count exceeds a preset threshold ε, it checks whether the maximum iteration count N has been reached. If the maximum iteration count N has not been reached, random sampling points are regenerated and the random expansion tree is further expanded. If the maximum iteration count N has been reached, the current path planning ends.

[0114] In one possible implementation, please refer to Figure 4The single-arm robotic RRT-APF path planning algorithm includes the following steps: initializing parameters and initializing a random expansion tree; sampling random points in the joint configuration space; constructing an artificial potential field function; synthesizing the node expansion direction based on the artificial potential field guidance direction and the random expansion direction; expanding new nodes along the synthesis direction; performing collision detection on the joint configuration corresponding to the new node; if a collision occurs, resampling is performed; if no collision occurs, it is further determined whether the minimum distance between the new node and the obstacle is less than a threshold. RRT-APF is a hybrid path search method that combines the random sampling expansion of the RRT algorithm with the potential field gradient guidance of the artificial potential field method. In the node expansion stage of the random expansion tree, this method uses preset fusion weights to weight and fuse the random expansion direction and the potential field guidance direction to determine the final expansion direction of the node. This allows the path search to maintain the global exploration capability of the RRT algorithm while obtaining the local guidance effect of the artificial potential field method towards the target and away from obstacles. The RRT algorithm is a sampling-based path planning algorithm that, by randomly sampling in the joint configuration space, allows a tree to quickly expand into unknown regions, thereby finding a feasible path in high-dimensional space. APF stands for Artificial Potential Field, which treats targets and obstacles as objects that exert attractive and repulsive forces on the robot, respectively, and the robot moves along the resultant force of these forces.

[0115] In this embodiment, global exploration is performed through random sampling and random tree expansion, preserving the algorithm's ability to find feasible paths when there are sufficient iterations. By weighted fusion of the random expansion direction and the gradient direction of the total potential field, node expansion maintains randomness while tending towards the target and deviating from obstacles, increasing the proportion of effective sampling nodes in narrow or non-convex dense spaces and reducing invalid expansion and backtracking. After node expansion, collision detection is performed based on the collision model to ensure the physical feasibility of the path. Specifically, by fusing the RRT algorithm and the artificial potential field method, the global path completeness of the RRT algorithm is preserved, and the potential field guidance solves the problems of slow convergence and low sampling efficiency in narrow spaces when using only the RRT algorithm. At the same time, it overcomes the defect of the pure artificial potential field method being prone to getting trapped in local optima, which can improve the efficiency of path planning and obstacle avoidance safety in complex non-convex environments.

[0116] In an exemplary embodiment, the step of constructing a collision model based on operational environment data includes:

[0117] Step C1: Construct a three-dimensional environment model based on the working environment data.

[0118] Step C2: Extract non-convex obstacle data and environment boundary data from the 3D environment model.

[0119] Step C3 involves performing convex decomposition on the non-convex obstacle data and environmental boundary data to obtain a combination of multiple approximate convex geometries corresponding to each.

[0120] Step C4: Construct a collision model based on the combination of multiple approximate convex geometries.

[0121] For example, for non-convex obstacles and non-convex environmental boundaries in the working environment, the corresponding non-convex geometric model can be divided into multiple approximate convex geometries that can be processed by convex body collision detection algorithms. A corresponding geometric feature data set is constructed based on the position, orientation, and geometry of each approximate convex geometries, and a collision model is built based on each geometric feature data set. For obstacles that are themselves convex, a collision model can be constructed directly using the convex geometric model corresponding to the obstacle, or a convex bounding body that matches the shape of the obstacle can be used to construct the collision model.

[0122] It is understandable that a collision model can simultaneously include convex geometry corresponding to convex obstacles, as well as a combination of multiple approximately convex geometries corresponding to non-convex obstacles and non-convex environment boundaries.

[0123] In one possible implementation, the V-HACD algorithm can be used to perform convex decomposition on the non-convex obstacle data and environmental boundary data extracted from the 3D environment model, decomposing the corresponding non-convex 3D model into multiple approximate convex geometries. A collision model is then constructed based on the position, orientation, and geometry of these approximate convex geometries. The V-HACD algorithm is an algorithm for approximate convex decomposition of 3D models. It uses variational hierarchical clustering to decompose complex non-convex models into multiple convex hulls to optimize collision detection and physics simulation performance. During path planning, multiple approximate convex geometries can be used as input to the GJK algorithm to detect whether a collision occurs between the robotic arm of the nuclear power robot and each approximate convex geometry. If no collision occurs, the minimum physical distance between the robotic arm and the corresponding approximate convex geometries is calculated. Taking a workspace enclosed by a hemispherical dome as an example, the V-HACD algorithm can be used to perform convex decomposition on the non-convex environmental boundary model corresponding to the hemispherical dome, obtaining multiple approximate convex geometries that jointly represent the inner wall of the dome and the semi-circular exit.

[0124] like Figure 5 As shown, Figure 5 The left side shows the original 3D model. Figure 5 The middle section is a model represented using a convex mesh collider. Figure 5The right side shows a combination of multiple approximate convex geometries obtained after convex decomposition using the V-HACD algorithm. The environment boundary collision model processed by V-HACD convex decomposition accurately preserves the spherical spatial configuration of the dome's inner wall and the shape of the semi-circular exit, consistent with the real structure; while the model directly extracted using convex hull occupies its internal space and loses the entrance and exit structures, resulting in compressed feasible space and false collisions and missed collisions. Therefore, convex decomposition can more accurately restore the real feasible space of complex non-convex environments.

[0125] In this embodiment, convex decomposition transforms non-convex obstacles and environmental boundaries into a combination of multiple approximate convex geometries. This avoids the compression of feasible space and false collisions caused by directly using convex envelopes for modeling, and more accurately recreates the real feasible space enclosed by non-convex obstacles and curved boundaries. This improves the accuracy of collision model construction and the effectiveness of subsequent path planning in complex non-convex environments. Specifically, considering the characteristics of numerous non-convex irregular obstacles and narrow spaces after a nuclear accident, the V-HACD convex decomposition algorithm is used to model complex environments. This enables autonomous robot operation without personnel entering high-radiation areas, improving personnel safety and environmental adaptability in nuclear emergency response.

[0126] In an exemplary embodiment, the step of planning the synchronous joint motion path of the main arm and the auxiliary arm according to the target motion trajectory of the target object includes:

[0127] Step D1: Determine the first rigid body transformation relationship between the target object's center coordinate system and the first grasping point coordinate system, and the second rigid body transformation relationship between the target object's center coordinate system and the second grasping point coordinate system.

[0128] For example, O can be set as the coordinate system of the center of the target object. The coordinate system of the grasping point corresponding to the main arm. This is the coordinate system for the gripping point of the secondary arm. After the main arm and secondary arm grip the target object, the positional relationship of the target object relative to the gripping points of the main arm and secondary arm remains unchanged. The coordinate systems of the gripping points can be determined separately. Homogeneous transformation matrix relative to the object's central coordinate system O and the coordinate system of the grab point Homogeneous transformation matrix relative to the object's central coordinate system O Since the target object is assumed to be a rigid body and the end effector is rigidly aligned with the corresponding gripping point, therefore and All are constant transformation relationships.

[0129] Step D2: Determine the pose sequences of the first grasping point and the second grasping point according to the pose sequence of the target object's center coordinate system along the target motion trajectory, the first rigid body transformation relationship, and the second rigid body transformation relationship.

[0130] For example, based on the transport task objectives and workspace environment constraints, a pose sequence of the target object's center coordinate system O along the target motion trajectory in the robot's base coordinate system W can be generated, with each pose represented by a homogeneous transformation matrix. The target motion trajectory includes the position trajectory of the target object's central coordinate system, or it may include both the position trajectory and the attitude trajectory. It is determined based on the pose of the target object's central coordinate system at each trajectory point. First rigid body transformation relationship Second rigid body transformation relationship Calculate the coordinate system of the main arm grasping point respectively. Pose sequence in robot base coordinate system and the coordinate system of the secondary arm's grasping point Pose sequence in robot base coordinate system This yields the pose sequences of the first and second gripping points as they move with the target object.

[0131] Step D3: Determine the end effector pose sequence of the main arm based on the relative pose relationship between the first gripping point and the end effector of the main arm, and determine the end effector pose sequence of the secondary arm based on the relative pose relationship between the second gripping point and the end effector of the secondary arm.

[0132] For example, it can be set The coordinate system of the end effector of the main arm. Let be the coordinate system of the end effector of the auxiliary arm. After the main arm and auxiliary arm clamp the target object, the coordinate system of the end effector of the main arm is... Relative to the coordinate system of the main arm grasping point homogeneous transformation matrix The coordinate system of the end effector of the auxiliary arm remains constant. Relative to the coordinate system of the secondary arm's grasping point homogeneous transformation matrix Maintain a constant coordinate system. (Based on the coordinate system of the main arm's gripping point) pose at each trajectory point and constant relative pose relationships Determine the end effector pose sequence of the main arm's end effector coordinate system E1 in the robot's base coordinate system. Similarly, based on the coordinate system of the secondary arm's grasping point... pose at each trajectory point and constant relative pose relationships Determine the coordinate system of the end effector of the auxiliary arm. End-effector pose sequence in robot base coordinate system .

[0133] Step D4: Based on the end-positional sequence of the main arm and the auxiliary arm, plan the synchronous joint motion path of the main arm and the auxiliary arm.

[0134] For example, the coordinate system of the end effector of the main arm can be... End-effector pose sequence in robot base coordinate system Inverse kinematics is performed on each pose to obtain the joint configuration of the main arm at each trajectory point. These joint configurations are then arranged in temporal order to generate the joint motion path of the main arm. Similarly, the coordinate system of the end effector of the auxiliary arm is used... End-effector pose sequence in robot base coordinate system Inverse kinematics is performed on each pose to obtain the joint configuration of the subarm at each trajectory point. The joint configurations are then arranged in time sequence to generate the joint motion path of the subarm. Based on the time synchronization relationship of the end-effector pose sequence, the joint motion paths of the main arm and the subarm are executed synchronously to obtain the synchronized joint motion paths of the main arm and the subarm.

[0135] Furthermore, based on the operational environment data, step D4 includes:

[0136] Step D41: Perform inverse kinematics solution on the end pose sequence of the main arm to generate the joint motion path of the main arm.

[0137] For example, the end effector coordinate system of the main arm can be obtained. End-effector pose sequence in robot base coordinate system W The end-effector pose sequence consists of the target poses of the end effector of the main arm at various time points during the movement of the target object along the target trajectory. For each pose in the end-effector pose sequence, inverse kinematics is performed based on the kinematic model of the main arm to obtain a set of joint variable values. All poses in the end-effector pose sequence are solved sequentially to obtain the joint variable value sequence of the main arm at each time point. The joint variable values ​​of the main arm at each time point are arranged in temporal order to generate the joint motion path of the main arm.

[0138] Step D42: Perform inverse kinematics on the end pose sequence of the auxiliary arm to generate the joint motion path of the auxiliary arm.

[0139] Similarly, the end effector coordinate system of the auxiliary arm can be obtained. End-effector pose sequence in robot base coordinate system W The end-effector pose sequence consists of the target pose of the end effector of the auxiliary arm at each moment during the movement of the target object along the target trajectory. For each pose in the end-effector pose sequence, inverse kinematics is solved based on the kinematic model of the auxiliary arm to obtain a set of joint variable values. All poses in the end-effector pose sequence are solved sequentially to obtain the joint variable value sequence of the auxiliary arm at each moment. The joint variable values ​​of the auxiliary arm at each moment are arranged in temporal order to generate the joint motion path of the auxiliary arm.

[0140] Step D43: Based on the time synchronization relationship between the joint motion paths of the main arm, the joint motion paths of the auxiliary arm, and the end-effector pose sequence, plan the synchronous joint motion paths of the main arm and the auxiliary arm.

[0141] For example, each pose in the end-effector pose sequence can correspond to a trajectory point under the same time reference. After generating the joint motion paths of the main arm and the auxiliary arm respectively, the time synchronization relationship of the end-effector pose sequence is used as the synchronization reference to time-align the joint motion paths of the main arm and the auxiliary arm, so that the main arm and the auxiliary arm arrive at their respective joint configurations at the same time. In one possible implementation, the joint motion paths of the main arm and the auxiliary arm can be interpolated and smoothed separately to obtain continuously executable joint motion trajectories. The joint motion commands of the main arm and the auxiliary arm are output synchronously under the same time reference, so that the main arm and the auxiliary arm execute synchronously along their respective joint motion paths.

[0142] In this embodiment, by determining the rigid body transformation relationship between the object's center coordinate system and the gripping point coordinate system, and based on the object's pose sequence along the target motion trajectory and the aforementioned rigid body transformation relationship, the gripping point pose sequence is determined. Furthermore, the end effector pose sequence is determined based on the relative pose relationship between the gripping point and the end effector. This ensures that the end effector's motion path is directly derived from the object's motion trajectory, guaranteeing rigid alignment between the end effectors and the gripping point, and reducing torsional or tensile torques on the object caused by inconsistent arm movements. Further, by performing inverse kinematics on the end effector pose sequences of the main arm and auxiliary arm respectively to generate corresponding joint motion paths, and by synchronizing the joint motion paths of the main arm and auxiliary arm according to the temporal synchronization relationship of the end effector pose sequences, the synchronous motion constraints in the task space are transferred to the joint space, ensuring temporal consistency of the arm movements in the joint space and improving the coordination of arm movements during collaborative handling.

[0143] In one exemplary embodiment, step 120 includes:

[0144] Step 1201: Establish link coordinate systems for the main arm and auxiliary arm of the nuclear power robot and determine the corresponding kinematic parameters.

[0145] For example, the main arm and the auxiliary arm can each be considered as a serial robot. The DH method is used to establish link coordinate systems for the main arm and the auxiliary arm respectively, determining four DH parameters between adjacent joint coordinate systems: link length, link rotation angle, link offset, and joint angle. The DH method is used to systematically establish a coordinate system for each link system of the robot and standardize the transformation relationship between adjacent link coordinate systems. The relative positions between adjacent link coordinate systems can be described using only these four parameters, simplifying robot kinematic modeling. For each joint, if it is a rotary joint, its joint variable is the joint angle; if it is a translational joint, its joint variable is the displacement. The link coordinate systems for the main arm and the auxiliary arm are established using the above DH parameters, and the corresponding kinematic parameters are determined. Here, the kinematic parameters include the DH parameters of each joint and the value range of each joint variable.

[0146] Step 1202: Based on the link coordinate system and kinematic parameters, construct the forward kinematic model of the main arm and the forward kinematic model of the secondary arm, respectively.

[0147] For example, a homogeneous transformation matrix between adjacent joint coordinate systems in the main arm can be constructed based on the DH parameters between each adjacent joint coordinate system of the main arm. This homogeneous transformation matrix describes the position and attitude transformation relationship between adjacent joint coordinate systems. Multiplying these homogeneous transformation matrices sequentially yields the overall transformation matrix of the main arm from the base coordinate system to the end effector coordinate system. This overall transformation matrix serves as the forward kinematics model of the main arm, used to calculate the position and attitude of the end effector in the robot's base coordinate system based on a set of joint variable values. Similarly, a homogeneous transformation matrix between adjacent joint coordinate systems in the secondary arm can be constructed based on the DH parameters between each adjacent joint coordinate system. Multiplying these homogeneous transformation matrices sequentially yields the overall transformation matrix of the secondary arm from the base coordinate system to the end effector coordinate system. This overall transformation matrix serves as the forward kinematics model of the secondary arm.

[0148] Step 1203: Discretely sample each joint variable of the main arm and the auxiliary arm within each joint angle range to obtain the sampled value of each joint variable.

[0149] For example, for each joint variable of the main arm, the value range of each joint variable can be discretized according to a preset sampling step size based on the joint angle range or displacement range corresponding to each joint variable, to obtain multiple sampled values ​​corresponding to each joint variable of the main arm. For example, for a certain rotary joint of the main arm, its joint angle range is... to Discrete sampling is performed on this range at preset sampling intervals to obtain the angle sampling sequence corresponding to the joint. Similarly, for each joint variable of the auxiliary arm, discrete sampling is performed in the same way within their respective joint angle range or displacement range to obtain multiple sample values ​​corresponding to each joint variable of the auxiliary arm.

[0150] Step 1204: Traverse the joint angle combinations within the range of each joint angle, substitute the sampled values ​​of each joint of the main arm into the forward kinematics model of the main arm, and obtain the set of reachable points of the main arm. The set of reachable points of the main arm is associated with the end reachable region of the main arm.

[0151] For example, the sampled values ​​of all joint variables of the main arm can be fully combined to obtain multiple sets of joint configuration state vectors. Each set of joint configuration state vectors contains a set of sampled values ​​of all joint variables of the main arm. By traversing all joint configuration state vectors, each set of joint configuration state vectors is substituted into the forward kinematics model of the main arm (i.e., the overall transformation matrix of the main arm from the base coordinate system to the end effector coordinate system) to calculate the position coordinates of the end effector of the main arm in the robot base coordinate system. The set of all position coordinates obtained after combining all joint angles is taken as the set of reachable points of the main arm. The area occupied by the set of reachable points in space is the reachable region of the end effector of the main arm.

[0152] Step 1205: Traverse the joint angle combinations within each joint angle range, substitute the sampled values ​​of each joint of the auxiliary arm into the auxiliary arm forward kinematics model, and obtain the auxiliary arm reachable point set. The auxiliary arm reachable point set is associated with the end reachable region of the auxiliary arm.

[0153] Similarly, the sampled values ​​of all joint variables of the auxiliary arm can be fully combined to obtain multiple sets of joint configuration state vectors. Each set of joint configuration state vectors contains a set of sampled values ​​of all joint variables of the auxiliary arm. By traversing all joint configuration state vectors, each set of joint configuration state vectors is substituted into the forward kinematics model of the auxiliary arm (i.e., the overall transformation matrix of the auxiliary arm from the base coordinate system to the end effector coordinate system) to calculate the position coordinates of the end effector in the robot's base coordinate system. The set of all position coordinates obtained after combining all joint angles is taken as the set of reachable points of the auxiliary arm. The region occupied by the set of reachable points in space is the reachable region of the end effector of the auxiliary arm.

[0154] Step 1206: Perform an intersection operation on the reachable point set of the main arm and the reachable point set of the secondary arm, and determine the collaborative reachable region based on the intersection operation result.

[0155] For example, the intersection of the main arm reachable point set and the secondary arm reachable point set can be calculated. This intersection includes all position coordinates that belong to both the main arm and the secondary arm reachable point sets. Based on the intersection calculation result, a cooperative reachable region is determined. This cooperative reachable region represents the spatial area that both the main arm and the secondary arm can reach and manipulate the target object. This cooperative reachable region provides constraints for the docking pose planning of the mobile chassis, ensuring that even if the target object is located within this cooperative reachable region, the target object is within the range that both the main arm and the secondary arm can effectively manipulate after the mobile chassis docks.

[0156] Furthermore, the steps for constructing the forward kinematics models of the main arm and the secondary arm respectively include:

[0157] Step E1: Determine the transformation relationship between adjacent joint coordinate systems in the main arm and the auxiliary arm, respectively.

[0158] For example, after establishing the link coordinate systems of the main arm and the auxiliary arm separately using the DH method, for each pair of adjacent joint coordinate systems of the main arm, the corresponding DH parameters (such as link length) can be used. Linkage angle Linkage offset Joint angle Construct the homogeneous transformation matrix between the adjacent joint coordinate systems. The homogeneous transformation matrix describes the transformation relationship from the (i-1)th joint coordinate system to the ith joint coordinate system, and its matrix form is:

[0159] (4)

[0160] in, Let be the joint angle of the i-th joint. Let be the link rotation angle of the i-th joint. Let be the link length of the i-th joint. Let be the link offset of the i-th joint. For each pair of adjacent joint coordinate systems of the main arm, determine the corresponding homogeneous transformation matrix. , where n is the number of joints in the main arm. Similarly, for each pair of adjacent joint coordinate systems in the secondary arm, a homogeneous transformation matrix between the adjacent joint coordinate systems is constructed based on the corresponding DH parameters.

[0161] Step E2: Combine the transformation relationships between the coordinate systems of each adjacent joint corresponding to the main arm and the auxiliary arm in sequence to obtain the overall transformation relationship from the base coordinate system to the end effector coordinate system.

[0162] For example, for the main arm, the homogeneous transformation matrices between the coordinate systems of each adjacent joint of the main arm can be multiplied sequentially from the base to the end effector to obtain the overall homogeneous transformation matrix of the main arm from the base coordinate system to the end effector coordinate system. , where n is the number of joints in the main arm. The overall homogeneous transformation matrix describes the position and orientation of the end effector of the main arm relative to the base coordinate system, and the value of this matrix is ​​uniquely determined by the current value of each joint variable of the main arm. Similarly, for the auxiliary arm, the homogeneous transformation matrices between the coordinate systems of each adjacent joint of the auxiliary arm are multiplied sequentially from the base to the end effector to obtain the overall homogeneous transformation matrix of the auxiliary arm from the base coordinate system to the end effector coordinate system.

[0163] Step E3: Use the overall transformation relationship corresponding to the main arm as the forward kinematic model of the main arm.

[0164] For example, the global homogeneous transformation matrix of the main arm from the base coordinate system to the end effector coordinate system can be used. The forward kinematics model of the main arm is determined. This forward kinematics model uses the joint configuration state vector of the main arm. For input, where This represents the joint variable value of the i-th joint of the main arm. The position and orientation of the end effector of the main arm in the robot base coordinate system are used as outputs to calculate the corresponding end position coordinates in subsequent steps based on the sampled values ​​of each joint of the main arm.

[0165] Step E4: Use the overall transformation relationship corresponding to the secondary arm as the forward kinematic model of the secondary arm.

[0166] For example, the overall homogeneous transformation matrix of the subarm from the base coordinate system to the end effector coordinate system can be determined as the forward kinematics model of the subarm. This forward kinematics model takes the joint configuration state vector of the subarm as input and the position and orientation of the end effector of the subarm in the robot base coordinate system as output, and is used to calculate the corresponding end position coordinates based on the sampled values ​​of each joint of the subarm in subsequent steps.

[0167] In this embodiment, by establishing a forward kinematics model and discretizing the joint variables, the reachable point sets of the main arm and auxiliary arm are obtained by traversing all joint angle combinations. This allows for a comprehensive acquisition of all spatial positions that the end effector of each robotic arm can reach under joint constraints. By performing an intersection operation on the reachable point sets of the main arm and auxiliary arm, a cooperative reachable region is obtained, which determines the spatial intersection range reachable by both arms, providing a spatial constraint basis for chassis docking pose planning. Furthermore, by determining the transformation relationship between adjacent joint coordinate systems and combining them sequentially, the overall transformation relationship from the base to the end effector is obtained, establishing a mapping relationship between joint configuration and end effector pose, providing a kinematic basis for calculating the reachable point set. Thus, through the determination of the cooperative reachable region and kinematic modeling, accessibility guarantees can be provided for chassis docking and dual-arm grasping.

[0168] In one exemplary embodiment, the method for determining the target docking pose includes:

[0169] Step F1: Based on the position information of the target object and the cooperative reachable area, determine the set of dockable poses of the mobile chassis. The dockable poses in the set of dockable poses are the poses that allow the target object to be located within the cooperative reachable area.

[0170] Step F2: Based on the preset optimization rules, select the target parking position of the mobile chassis from the set of parking positions.

[0171] For example, the evaluation value of each dockable pose can be determined based on the path length of the mobile chassis moving from the current position to each dockable pose, the distance of the target object relative to the boundary of the reachable area of ​​the two arms, and the operability of the two arms in the corresponding dockable pose, and the target docking pose can be selected based on the evaluation value.

[0172] In this embodiment, by determining the set of dockable poses with the cooperative reachable area as a constraint and selecting the target docking pose from it, the chassis docking position can ensure that the target object is within the operable range of the two arms, reducing the situation where the two arms cannot reach the target object due to docking deviation or the chassis position needs to be repeatedly adjusted, thereby improving the initial positioning efficiency of the grasping operation.

[0173] In one possible implementation, please refer to Figure 6 , Figure 6 This is a schematic diagram of a multi-stage hybrid path planning framework in one embodiment. Figure 6 Part (a) shows the preparation phase before the dual-arm grasping action. In this phase, the target object is positioned within a space accessible to both the main and auxiliary arms through collaborative reach area analysis, and the parking posture of the mobile chassis is determined based on the collaborative reach area. Figure 6 Part (b) illustrates the sequential grasping path planning stage of the two arms. In this stage, the main arm first plans the grasping path from the current joint configuration to the corresponding target grasping pose. After the main arm reaches the target pose, the secondary arm then plans the grasping path from the current joint configuration to the corresponding target grasping pose. Figure 6 Part (c) illustrates the dual-arm coordinated lifting path planning stage based on the object center trajectory. Specifically, with the target object center coordinate system as a reference, the end effector pose sequences corresponding to the end effectors of the main arm and the auxiliary arm are determined respectively. After inverse kinematics solving, the joint configuration sequences of the main arm and the auxiliary arm are obtained respectively, and a smooth synchronous joint motion path is generated through interpolation.

[0174] It should be understood that although the steps in the flowcharts of the embodiments described above are shown sequentially according to the arrows, these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated herein, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Moreover, at least some steps in the flowcharts of the embodiments described above may include multiple steps or multiple stages. These steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these steps or stages is not necessarily sequential, but can be performed alternately or in turn with other steps or at least some of the steps or stages in other steps. It is understood that the steps in different embodiments can be freely combined as needed, and all non-contradictory solutions formed by such combinations are within the scope of protection of this application.

[0175] Based on the same inventive concept, this application also provides a robotic arm path planning device for nuclear power robots to implement the above-mentioned robotic arm path planning method. The solution provided by this device is similar to the solution described in the above method. Therefore, the specific limitations of one or more embodiments of the robotic arm path planning device for nuclear power robots provided below can be found in the limitations of the robotic arm path planning method for nuclear power robots described above, and will not be repeated here.

[0176] In one exemplary embodiment, such as Figure 7 As shown, a path planning device 200 for a nuclear power robot's robotic arm is provided, comprising: an acquisition module 201, an reachable area determination module 202, a motion parameter determination module 203, and a path planning module 204, wherein:

[0177] The acquisition module 201 is used to acquire the working environment data of the nuclear power robot and construct a collision model based on the working environment data. The collision model includes the geometric feature data of obstacles in the working environment and the boundary of the working environment.

[0178] The reachable area determination module 202 is used to determine the cooperative reachable area of ​​the main arm and the auxiliary arm based on the joint configuration data of the main arm and the auxiliary arm of the nuclear power robot.

[0179] The motion parameter determination module 203 is used to determine the target docking posture of the mobile chassis of the nuclear power robot and the target joint configuration of the main arm and the auxiliary arm respectively, based on the position information of the target object and the cooperative reachable area.

[0180] The path planning module 204 is used to, when the mobile chassis moves to the target docking pose, sequentially treat one of the secondary arm and the main arm as an obstacle, update the collision model, and plan the motion path of the other of the secondary arm and the main arm from the current joint configuration to the corresponding target joint configuration based on the updated collision model; wherein, the motion path is determined by fusing the random expansion direction and the potential field guidance direction, the random expansion direction is the direction of the node in the random expansion tree pointing to the random sampling point, the potential field guidance direction is the opposite direction of the gradient direction of the total potential field, and the total potential field is determined based on the updated collision model and the target joint configuration;

[0181] The path planning module 204 is also used to plan the synchronous joint motion path of the main arm and the auxiliary arm according to the target motion trajectory of the target object when the main arm and the auxiliary arm move to their respective target joint configurations and clamp the target object.

[0182] In one embodiment, the path planning module 204 is further configured to:

[0183] When the mobile chassis moves to the target parking position, the auxiliary arm is added as an obstacle to the collision model according to the current joint configuration of the auxiliary arm to update the collision model. Based on the updated collision model, the motion path of the main arm from the current joint configuration to the corresponding target joint configuration is planned.

[0184] When the main arm moves to the corresponding target joint configuration, the main arm is added as an obstacle to the collision model according to the target joint configuration of the main arm to update the collision model. Based on the updated collision model, the motion path of the secondary arm from the current joint configuration to the corresponding target joint configuration is planned.

[0185] In one embodiment, the path planning module 204 is further configured to:

[0186] Obtain the initial joint configuration and preset expansion step size, and construct a random expansion tree with the initial joint configuration as the root node;

[0187] Random sampling points are generated in the joint configuration space, and the node closest to the random sampling point is found in the current random expansion tree as the nearest node. The joint configuration space is determined by the angle range of each joint in the target joint configuration and the initial joint configuration. The points in the joint configuration space are used to represent a set of joint variable values.

[0188] Using the target joint configuration as the attraction source and the obstacle as the repulsion source, an attraction potential field and a repulsion potential field are constructed, and superimposed to obtain the total potential field. The gradient direction of the total potential field at the nearest node is then calculated.

[0189] The direction from the nearest node to the random sampling point is taken as the random expansion direction, and the opposite direction of the total potential gradient is taken as the potential field guiding direction. The random expansion direction and the potential field guiding direction are weighted and fused by a preset fusion weight to determine the target expansion direction. New nodes are generated along the target expansion direction according to the expansion step size.

[0190] Collision detection is performed on the joint configuration corresponding to the new node based on the updated collision model. If the collision detection result is that no collision has occurred, the new node is added to the random expansion tree.

[0191] If the distance between the new node and the target joint configuration is less than or equal to a preset threshold, the motion path from the initial joint configuration to the target joint configuration is output.

[0192] In one embodiment, the acquisition module 201 is further configured to:

[0193] Based on the operational environment data, construct a three-dimensional environment model;

[0194] Extracting non-convex obstacle data and environment boundary data from a 3D environment model;

[0195] Convex decomposition is performed on non-convex obstacle data and environmental boundary data to obtain a combination of multiple approximate convex geometries corresponding to each.

[0196] A collision model is constructed based on a combination of multiple approximate convex geometries.

[0197] In one embodiment, the path planning module 204 is further configured to:

[0198] Determine the first rigid body transformation relationship between the target object's center coordinate system and the first gripping point coordinate system, and the second rigid body transformation relationship between the target object's center coordinate system and the second gripping point coordinate system;

[0199] Based on the pose sequence of the target object's center coordinate system along the target's motion trajectory, the first rigid body transformation relationship, and the second rigid body transformation relationship, determine the pose sequences of the first grasping point and the second grasping point respectively.

[0200] Based on the relative pose relationship between the first gripping point and the end effector of the main arm, the end effector pose sequence of the main arm is determined, and based on the relative pose relationship between the second gripping point and the end effector of the secondary arm, the end effector pose sequence of the secondary arm is determined.

[0201] Based on the end-positional sequence of the main arm and the auxiliary arm, plan the synchronous joint motion path of the main arm and the auxiliary arm.

[0202] In one embodiment, the path planning module 204 is further configured to:

[0203] Inverse kinematics is performed on the end-effector pose sequence of the main arm to generate the joint motion path of the main arm;

[0204] Inverse kinematics is performed on the end-effector pose sequence to generate the joint motion path of the arm.

[0205] Based on the time synchronization relationship between the joint motion paths of the main arm, the joint motion paths of the auxiliary arm, and the end-effector pose sequence, the synchronous joint motion paths of the main arm and the auxiliary arm are planned.

[0206] In one embodiment, the reachable area determination module 202 is further configured to:

[0207] Establish link coordinate systems for the main arm and auxiliary arm of the nuclear power robot and determine the corresponding kinematic parameters;

[0208] Based on the link coordinate system and kinematic parameters, the forward kinematic models of the main arm and the secondary arm are constructed respectively.

[0209] Discrete sampling is performed on each joint variable of the main arm and the auxiliary arm within each joint angle range to obtain the sampled value of each joint variable;

[0210] By iterating through the joint angle combinations within each joint angle range, the sampled values ​​of each joint of the main arm are substituted into the main arm forward kinematics model to obtain the main arm reachable point set, which is associated with the end reachable region of the main arm.

[0211] By iterating through the joint angle combinations within each joint angle range, the sampled values ​​of each joint of the auxiliary arm are substituted into the positive kinematics model of the auxiliary arm to obtain the set of reachable points of the auxiliary arm. The set of reachable points of the auxiliary arm is associated with the end reachable region of the auxiliary arm.

[0212] Perform an intersection operation on the reachable point sets of the main arm and the secondary arm, and determine the collaborative reachable region based on the intersection operation result.

[0213] In one embodiment, the reachable area determination module 202 is further configured to:

[0214] Determine the transformation relationship between the coordinate systems of adjacent joints in the main arm and the auxiliary arm respectively;

[0215] The transformation relationships between the coordinate systems of each adjacent joint corresponding to the main arm and the auxiliary arm are combined sequentially to obtain the overall transformation relationship from the base coordinate system to the end effector coordinate system;

[0216] The overall transformation relationship corresponding to the main arm is taken as the positive kinematic model of the main arm;

[0217] The overall transformation relationship corresponding to the secondary arm is used as the forward kinematic model of the secondary arm.

[0218] In one embodiment, the motion parameter determination module 203 is further configured to:

[0219] Based on the location information of the target object and the cooperative reachable area, determine the set of dockable poses of the mobile chassis. The dockable poses in the set of dockable poses are the poses that allow the target object to be located within the cooperative reachable area.

[0220] Based on preset optimization rules, the target parking position of the mobile chassis is selected from the set of available parking positions.

[0221] The various modules in the robotic arm path planning device of the aforementioned nuclear power robot can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device in hardware form, or stored in the memory of a computer device in software form, so that the processor can call and execute the operations corresponding to each module.

[0222] In one exemplary embodiment, a computer device is provided, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps in the above-described method embodiments.

[0223] In one embodiment, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the steps in the above method embodiments.

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

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

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

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

Claims

1. A method for path planning of a robotic arm in a nuclear power plant, characterized in that, The method includes: Acquire the operating environment data of the nuclear power robot, and construct a collision model based on the operating environment data. The collision model includes the geometric feature data of obstacles in the operating environment and the boundary of the operating environment. Based on the joint configuration data of the main arm and the auxiliary arm of the nuclear power robot, the cooperative reachable area of ​​the main arm and the auxiliary arm is determined; Based on the location information of the target object and the cooperative reachable area, the target docking posture of the mobile chassis of the nuclear power robot and the target joint configuration of the main arm and the auxiliary arm are determined. When the mobile chassis moves to the target docking position, one of the secondary arm and the main arm is sequentially designated as an obstacle, the collision model is updated, and the motion path of the other of the secondary arm and the main arm from the current joint configuration to the corresponding target joint configuration is planned according to the updated collision model. The motion path is determined by fusing random expansion direction and potential field guidance direction. The random expansion direction is the direction from the node in the random expansion tree to the random sampling point, and the potential field guidance direction is the opposite direction of the total potential field gradient direction. The total potential field is determined according to the updated collision model and the target joint configuration. When the main arm and the auxiliary arm move to their respective target joint configurations and clamp the target object, the synchronous joint movement paths of the main arm and the auxiliary arm are planned according to the target movement trajectory of the target object.

2. The method according to claim 1, characterized in that, The step of sequentially treating one of the secondary arm and the primary arm as an obstacle, updating the collision model, and planning the motion path of the other of the secondary arm and the primary arm from the current joint configuration to the corresponding target joint configuration based on the updated collision model includes: When the mobile chassis moves to the target docking position, the auxiliary arm is added as an obstacle to the collision model according to the current joint configuration of the auxiliary arm, so as to update the collision model. Based on the updated collision model, the motion path of the main arm from the current joint configuration to the corresponding target joint configuration is planned. When the main arm moves to the corresponding target joint configuration, the main arm is added as an obstacle to the collision model according to the target joint configuration of the main arm to update the collision model, and the motion path of the secondary arm from the current joint configuration to the corresponding target joint configuration is planned according to the updated collision model.

3. The method according to claim 1, characterized in that, The methods for planning the motion path include: Obtain the initial joint configuration and the preset expansion step size, and construct a random expansion tree with the initial joint configuration as the root node; Random sampling points are generated in the joint configuration space, and the node closest to the random sampling point is found in the current random expansion tree as the nearest node. The joint configuration space is determined by the angle range of each joint where the target joint configuration and the initial joint configuration are located. The points in the joint configuration space are used to represent a set of joint variable values. Using the target joint as the attraction source and the obstacle as the repulsion source, an attraction potential field and a repulsion potential field are constructed, superimposed to obtain a total potential field, and the gradient direction of the total potential field at the nearest node is calculated. The direction from the nearest node to the random sampling point is taken as the random expansion direction, and the opposite direction of the total potential gradient direction is taken as the potential field guiding direction. The random expansion direction and the potential field guiding direction are weighted and fused by a preset fusion weight to determine the target expansion direction. New nodes are generated along the target expansion direction according to the expansion step size. Collision detection is performed on the joint configuration corresponding to the new node according to the updated collision model. If the collision detection result is that no collision has occurred, the new node is added to the random expansion tree. If the distance between the new node and the target joint configuration is less than or equal to a preset threshold, the motion path from the initial joint configuration to the target joint configuration is output.

4. The method according to claim 1, characterized in that, The step of constructing a collision model based on the operating environment data includes: Based on the described work environment data, a three-dimensional environment model is constructed; Extract non-convex obstacle data and environment boundary data from the three-dimensional environment model; The non-convex obstacle data and the environmental boundary data are subjected to convex decomposition processing to obtain a combination of multiple approximate convex geometries corresponding to each. A collision model is constructed based on the combination of the multiple approximate convex geometries.

5. The method according to claim 1, characterized in that, The step of planning the synchronous joint motion path of the main arm and the auxiliary arm based on the target motion trajectory of the target object includes: Determine the first rigid body transformation relationship between the target object center coordinate system and the first gripping point coordinate system, and the second rigid body transformation relationship between the target object center coordinate system and the second gripping point coordinate system; Based on the pose sequence of the target object's center coordinate system along the target's motion trajectory, the first rigid body transformation relationship, and the second rigid body transformation relationship, determine the pose sequences of the first grasping point and the second grasping point respectively; Based on the relative pose relationship between the first gripping point and the end effector of the main arm, the end effector pose sequence of the main arm is determined, and based on the relative pose relationship between the second gripping point and the end effector of the secondary arm, the end effector pose sequence of the secondary arm is determined. Based on the end-effector pose sequences of the main arm and the auxiliary arm, the synchronous joint motion paths of the main arm and the auxiliary arm are planned.

6. The method according to claim 5, characterized in that, The step of planning the synchronous joint motion path of the main arm and the auxiliary arm based on their respective end-effector pose sequences includes: Inverse kinematics is performed on the end-effector pose sequence of the main arm to generate the joint motion path of the main arm; Inverse kinematics is performed on the end-effector pose sequence to generate the joint motion path of the arm. Based on the time synchronization relationship between the joint motion paths of the main arm, the joint motion paths of the auxiliary arm, and the end-effector pose sequence, the synchronized joint motion paths of the main arm and the auxiliary arm are planned.

7. The method according to claim 1, characterized in that, The step of determining the cooperative reachable area of ​​the main arm and the auxiliary arm based on their respective joint configuration data includes: Establish link coordinate systems for the main arm and auxiliary arm of the nuclear power robot respectively and determine the corresponding kinematic parameters; Based on the link coordinate system and the kinematic parameters, the forward kinematic model of the main arm and the forward kinematic model of the auxiliary arm are constructed respectively. Discrete sampling is performed on each joint variable of the main arm and the auxiliary arm within each joint angle range to obtain the sampled value of each joint variable; By traversing the joint angle combinations within the range of each joint angle, the sampled values ​​of each joint of the main arm are substituted into the forward kinematics model of the main arm to obtain the set of reachable points of the main arm, which is associated with the end reachable region of the main arm. By traversing the joint angle combinations within the range of each joint angle, the sampled values ​​of each joint of the auxiliary arm are substituted into the positive kinematics model of the auxiliary arm to obtain the set of reachable points of the auxiliary arm, which is associated with the end reachable region of the auxiliary arm. Perform an intersection operation on the set of reachable points of the main arm and the set of reachable points of the secondary arm, and determine the cooperative reachable region based on the result of the intersection operation.

8. The method according to claim 7, characterized in that, The construction of the forward kinematics models of the main arm and the secondary arm includes: Determine the transformation relationship between adjacent joint coordinate systems in the main arm and the auxiliary arm respectively; The transformation relationships between the coordinate systems of each adjacent joint corresponding to the main arm and the auxiliary arm are combined sequentially to obtain the overall transformation relationship from the base coordinate system to the end effector coordinate system; The overall transformation relationship corresponding to the main arm is taken as the positive kinematic model of the main arm; The overall transformation relationship corresponding to the secondary arm is taken as the forward kinematic model of the secondary arm.

9. The method according to claim 1, characterized in that, The method for determining the target docking pose includes: Based on the location information of the target object and the cooperative reachable area, a set of dockable poses for the mobile chassis is determined, wherein the dockable poses in the set of dockable poses are poses that allow the target object to be located within the cooperative reachable area. According to preset optimization rules, the target parking position of the mobile chassis is selected from the set of parking positions.

10. A path planning device for the robotic arm of a nuclear power plant robot, characterized in that, The device includes: The acquisition module is used to acquire the working environment data of the nuclear power robot and construct a collision model based on the working environment data. The collision model includes the geometric feature data of obstacles in the working environment and the boundary of the working environment. The reachable area determination module is used to determine the cooperative reachable area of ​​the main arm and the auxiliary arm based on the joint configuration data of the main arm and the auxiliary arm of the nuclear power robot. The motion parameter determination module is used to determine the target docking posture of the mobile chassis of the nuclear power robot and the target joint configuration of the main arm and the auxiliary arm, based on the position information of the target object and the cooperative reachable area. The path planning module is used to, when the mobile chassis moves to the target docking pose, sequentially treat one of the secondary arm and the main arm as an obstacle, update the collision model, and plan the motion path of the other of the secondary arm and the main arm from the current joint configuration to the corresponding target joint configuration based on the updated collision model; wherein, the motion path is determined by fusing random expansion direction and potential field guidance direction, the random expansion direction is the direction from the node in the random expansion tree to the random sampling point, the potential field guidance direction is the opposite direction of the total potential field gradient direction, and the total potential field is determined based on the updated collision model and the target joint configuration; The path planning module is further configured to plan the synchronous joint motion path of the main arm and the auxiliary arm according to the target motion trajectory of the target object when the main arm and the auxiliary arm move to their respective target joint configurations and clamp the target object.

Images