A method and system for extending the motion trajectory of a robotic arm of an LGP

Abstract

The present invention discloses a method and system for extending the motion trajectory of a robotic arm of an LGP. The motion trajectory of the RU searching robotic arm is modeled for motion path planning through data optimization. Based on the modeling of the motion path planning, a reachability graph is constructed. In the path query system based on the LGP and the reachability graph, it is updated along with the feedback during the movement of the robotic arm, and the solutions with changed costs are deleted, and all the paths and intermediate nodes between two nodes, as well as the costs therein, are stored simultaneously. In the path query system based on the LGP and the reachability graph, the head and tail coordinates of each path are set as the starting point and the ending point, and the optimized path is transmitted to the control system of the robotic arm for execution. During the execution process, the state of the machine and environmental changes are monitored in real time, and dynamic adjustment is carried out when necessary.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arm motion trajectories, and in particular, to a method and system for extending the motion trajectory of a robotic arm of LGP. Background Art

[0002] In an intelligent factory, the motion efficiency and obstacle avoidance ability of a robotic arm greatly affect the production efficiency. The motion efficiency of the robotic arm directly determines the connection speed of each link on the production line and the processing rate of products, while the obstacle avoidance ability is related to the safety and stability of the robotic arm in the production environment. Therefore, a suitable motion planning is extremely crucial.

[0003] In the current robotic motion planning solutions, Logic-geometric programming (LGP) has gradually become one of the mainstream solutions for motion planning due to its good optimization processing ability shown in planning problems involving abstraction and geometric constraints.

[0004] However, in the process of implementing the technical solutions of the present invention in the embodiments of the present application, the inventors of the present application found that the above technologies have at least the following technical problems: when LGP processes non-planar systems such as mobile robotic arms, it cannot be well extended and there are obstacle avoidance problems. Summary of the Invention

[0005] The embodiments of the present application provide a method and system for extending the motion trajectory of a robotic arm of LGP, which solve the problems in the prior art that LGP cannot be well extended and there are obstacle avoidance problems when processing non-planar systems such as mobile robotic arms, and realize the ability to solve optimal tasks and trajectory optimization on high-degree-of-freedom mobile robotic arms.

[0006] The embodiments of the present application provide a method for extending the motion trajectory of a robotic arm of LGP. In the way of abstracting task and motion planning problems by LGP logic-geometric programming,

[0007] S1, model the motion path planning of finding the motion trajectory of the robotic arm by data optimization for the RU, where the RU is a rack unit; [[ID=2,7]]

[0008] S2, based on the modeling of the motion path planning, construct a reachability graph. The reachability graph is provided with nodes and edges. The nodes represent the positions of the end effectors of the mobile manipulator, and the edges represent the reachability between the nodes. Sufficient geometric constraints are provided by combining obstacles and existing production environment information through node verification and edge inspection, and the infeasible action sequences in the continuous domain are pruned by a heuristic method, so as to ensure feasibility in the final complete path trajectory optimization. After the construction of the reachability graph is completed;

[0009] S3. Based on the constructed reachability graph, in the path query system based on LGP and the reachability graph, it is updated along with the feedback during the movement of the robotic arm, and the solutions with changed costs are deleted. At the same time, all paths and intermediate nodes between two nodes, as well as the costs therein, are stored.

[0010] S4. In the path query system based on LGP and the reachability graph, set the start and end coordinates of each path as the starting point and the ending point. The reachability graph planner receives the start and end coordinates of each path as the starting point and the ending point, and uses Dijkstra to find the shortest path between the starting point and the ending point in the reachability graph. Verify whether there is a collision according to the distance of the simulated robotic arm movement. If there is a collision, save it to the solution library; after removing the edges with collisions from the reachability graph, re-plan. Similarly, when the edges connecting nodes are removed, the process of supplementing additional intermediate points will also be triggered, which is an iterative process; transfer the optimized path to the control system of the robotic arm for execution. During the execution process, monitor the status of the machine and environmental changes in real time, and make dynamic adjustments if necessary.

[0011] Further, in step S1, it also includes

[0012] The formula for data optimization is as follows:

[0013] ;

[0014] Among them, x represents a series of objects of the movement path, that is, the nodes in the reachability graph, a represents the constraint obstacles and the movement angle limit, g(x,a) and h(x,a) represent the constraints of x in different dimensions, and K is a coefficient.

[0015] Further, in step S1, it also includes

[0016] S11. Determine a symbolic task execution order. The first-layer search is symbolic, without directly considering the kinematics and environmental constraints of the robot, and generate a symbolic sequence.

[0017] S12. Convert the symbolic sequence generated by the first layer of the LGP into a series of key frames. By optimizing these key frames, LGP can consider the kinematic limitations and environmental constraints of the robot while maintaining the task logic order, so as to generate a task execution plan.

[0018] Further, in step S2, the construction of the reachability graph also includes

[0019] Node sampling module, which is responsible for randomly sampling nodes in the task space. These nodes represent the possible positions that the end effector of the robot can reach. This sampling process is uniformly and randomly distributed, so it is sufficient to cover the entire task space. The robotic arm model is defined as a local operating system with a floating base and a torso, and is represented by the following formula:

[0020] ;

[0021] where q represents the configuration of the robotic arm, q base is the position and orientation of the base, is the base node of the coordinates (x, y), is the orientation of the base node coordinates, q manipulator is the joint angle vector of the manipulator, q1, q2, q3,..., q m represents the angle vector of each joint of the manipulator.

[0022] Furthermore, in step S2, the construction of the reachability graph further includes

[0023] Edge connection module, which is performed after node sampling to establish effective connections between the nodes within the reach of the robotic arm, thereby generating potential motion paths of the robotic arm;

[0024] Each edge in the edge connection will be assigned a cost value, which can be based on the distance between nodes, the expected motion time, energy consumption, or other metrics related to the path quality;

[0025] Its cost formula is as follows:

[0026] ;

[0027] In the formula, C e is the cost of the edge, and are the positions of the two endpoints of the edge in the task space respectively, and are the robot configurations corresponding to these two positions, , and are the weight coefficients corresponding to position, configuration, and collision cost, and are the collision cost values of the nodes.

[0028] A system for extending the motion trajectory of a robotic arm of LGP, including

[0029] A motion path planning and modeling module is used to model the motion path planning of the RU search robotic arm through data optimization. The RU is a rack unit;

[0030] An accessibility graph construction module is used to construct an accessibility graph based on the modeling of the motion path planning. Nodes and edges are provided in the accessibility graph. The nodes represent the positions of the end effectors of the mobile manipulator, and the edges represent the accessibility between the nodes. Sufficient geometric constraints are provided by combining node verification and edge inspection with obstacles and existing production environment information. Infeasible action sequences in the continuous domain are pruned through heuristic methods, so as to ensure feasibility in the final complete path trajectory optimization. The construction of the accessibility graph is completed;

[0031] A path query module is used to update based on the feedback during the movement of the robotic arm in a path query system based on LGP and the accessibility graph, delete the solutions with changed costs, and simultaneously store all the paths and intermediate nodes between two nodes, as well as the costs therein;

[0032] A path planning module is used in a path query system based on LGP and the accessibility graph. Set the start and end coordinates of each path as the starting point and the ending point. The accessibility graph planner receives the start and end coordinates of each path as the starting point and the ending point, and uses Dijkstra to find the shortest path between the starting point and the ending point in the accessibility graph. Verify whether there is a collision according to the distance of the simulated robotic arm movement. If there is a collision, save it to the solution library; After removing the edges with collisions from the accessibility graph, re-plan. Similarly, removing the edges connected between nodes will also trigger the process of supplementing additional intermediate points, which is an iterative process; Transmit the optimized path to the control system of the robotic arm for execution. During the execution process, monitor the status of the machine and environmental changes in real time, and make dynamic adjustments if necessary.

[0033] Further, in the motion path planning and modeling module, it also includes

[0034] The formula for the data optimization is as follows:

[0035] ;

[0036] Among them, x represents a series of objects of the movement path, that is, the nodes in the accessibility graph, a represents the constraint obstacles and movement angle limits, g(x,a) and h(x,a) represent the constraints of x in different dimensions, and K is a coefficient.

[0037] Further, in the motion path planning and modeling module, it also includes,

[0038] A sequence decision-making layer unit, which is used to determine a symbolic task execution sequence. The search in the first layer is symbolic, without directly considering the kinematics and environmental constraints of the robot, and generates a symbolic sequence;

[0039] An optimized key-frame unit, which is used to convert the symbolic sequence generated by the first layer of the LGP into a series of key frames. By optimizing these key frames, the LGP can consider the kinematic constraints of the robot and environmental constraints while maintaining the logical order of the tasks, so as to generate a task execution plan.

[0040] Furthermore, in the path query module, it also includes,

[0041] A node sampling module, which is responsible for randomly sampling nodes in the task space. These nodes represent the possible positions that the end effector of the robot can reach. This sampling process is evenly and randomly distributed, so it is sufficient to cover the entire task space. The manipulator model is defined as a local operating system with a floating base and a torso, and is represented by the following formula:

[0042] ;

[0043] where q represents the configuration of the manipulator, q base is the position and orientation of the base, is the base node of the coordinates (x, y), is the orientation of the base node coordinates, q manipulator is the joint angle vector of the manipulator, q1, q2, q3,..., q m represents the angle vector of each joint of the manipulator.

[0044] Furthermore, in the path query module, it also includes,

[0045] An edge connection module, which is performed after node sampling to establish effective connections between the nodes within the reach of the manipulator, so as to generate potential motion paths of the manipulator;

[0046] Each edge in the edge connection will be assigned a cost value, which can be based on the distance between nodes, the expected motion time, energy consumption, or other metrics related to the path quality;

[0047] Its cost formula is as follows:

[0048] ;

[0049] In the formula, C e is the cost of the edge, and are the positions of the two endpoints of the edge in the task space respectively, and They are the robot configurations corresponding to these two positions. , and are the weight coefficients corresponding to the position, configuration, and collision cost. and are the collision cost values of the nodes.

[0050] One or more technical solutions provided in the embodiments of the present application have at least the following technical effects or advantages:

[0051] 1. Optimize high-degree-of-freedom mobile manipulators such as robotic arms, which can effectively handle complex kinematic problems and large-scale state spaces. In contrast, some traditional methods are often only suitable for single-dimensional planar movement and may face challenges in scalability and computational efficiency.

[0052] 2. By introducing a sampling-based reachability graph, R-LGP (which means Extended Logic Geometric Planning) can incorporate environmental information (such as obstacles) and provide sufficient geometric constraints, which helps generate feasible action sequences, reduce the collision risk, and improve the search efficiency. In contrast, previous solutions mostly adopted simplified environmental models, which may require re-planning during the movement process.

[0053] 3. Adopt a hierarchical method to optimize the planning and motion trajectory in stages. This structured method helps solve complex problems and improve the feasibility of planning. In contrast, many previous solutions only optimized one of the task planning or motion planning for the TAMP problem. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is a flowchart of the method for the robotic arm motion trajectory of the extended LGP;

[0055] Figure 2 It is a system architecture diagram of the robotic arm motion trajectory of the extended LGP;

[0056] Figure 3 It is a path query system diagram based on logic geometric planning and reachability graph. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0057] The present invention is a robotic arm motion trajectory optimization system. By combining Logic Geometric Planning (LGP) and a sampling-based reachability graph, it optimizes the hybrid motion planning involving abstract and geometric constraints.

[0058] Specifically, the present invention first converts the motion planning TAMP problem (where TAMP means Robot Task and Motion Planning) into a data planning problem and finds a local optimal solution through an optimization scheme.

[0059] Subsequently, a sampling-based reachability graph was proposed, which combines environmental information such as obstacles to construct geometric constraints.

[0060] Then, using the heuristic information provided by the reachability graph, infeasible paths are pruned to reduce the need for replanning in the final motion trajectory optimization.

[0061] Finally, the reachability graph is used for complete path optimization to ensure that the path meets geometric and kinematic constraints.

[0062] To better understand the above technical solution, the above technical solution will be described in detail below in conjunction with the accompanying drawings of the specification and specific implementation manners.

[0063] See Figure 1 , a method for the motion trajectory of a robotic arm to extend LGP. In the way of abstracting the task and motion planning problems in LGP logical geometric planning,

[0064] S1. Model the motion path planning of the RU search robotic arm through data optimization, where the RU is the rack unit;

[0065] Specifically, first, based on the way of abstracting the task and motion planning problems in LGP logical geometric planning, the motion trajectory of the RU search robotic arm is defined as a data optimization problem, and the formula is as follows:

[0066] ;

[0067] Among them, x represents a series of objects on the moving path, that is, the nodes in the reachability graph, a represents constraints (such as obstacles, moving angle limitations, etc.), g(x,a) and h(x,a) represent the constraints of x in different dimensions, and K is a coefficient.

[0068] In an intelligent factory, the objects and their constraints on the path mainly include the physical limitations of the machine itself, the position of the machine, and the obstacle information collected by sensors and cameras for modeling.

[0069] In the mathematical optimization design of LGP (Logical Geometric Programming), the task and motion planning problems are divided into three levels for solution. The first level is the compliant sequential decision-making layer. The goal of this layer is to determine a symbolic task execution sequence, which can be utilized by the motion planning algorithms in the subsequent levels. The search in the first layer is symbolic and does not directly consider the kinematics of the robot and environmental constraints, but it is necessary to ensure that the generated task sequence is logically coherent. The second layer focuses on optimizing key frames, that is, the important poses or configurations during the task execution, such as grasping poses or placement positions. In this layer, LGP converts the symbolic sequence generated in the first layer into a series of key frames. By optimizing these key frames, LGP can consider the kinematic limitations of the robot and environmental constraints while maintaining the logical order of the tasks, thereby generating a more feasible and effective task execution plan. The third layer is the complete path optimization, which optimizes the entire task execution path, including the complete motion trajectory of the robot from the initial state to the final state. The goal of this layer is to generate a complete path that not only meets the task requirements but also is kinematically feasible and efficient, while avoiding collisions and satisfying the physical limitations of the robot.

[0070] S2. Based on the modeling of the motion path planning, a reachability graph is constructed. The reachability graph has nodes and edges. The nodes represent the positions of the end effectors of the mobile manipulator, and the edges represent the reachability between the nodes. By combining node verification and edge inspection with obstacle and existing production environment information, sufficient geometric constraints are provided. The infeasible action sequences in the continuous domain are pruned by a heuristic method, thereby ensuring feasibility in the final complete path trajectory optimization. The construction of the reachability graph is completed.

[0071] Specifically, the reachability graph construction module is mainly used to collect obstacles based on the existing production environment, analyze the reachability of object nodes, construct a reachability graph, and provide sufficient geometric constraints for path optimization.

[0072] The nodes in the reachability graph represent the positions of the end effectors of the mobile manipulator, and the edges represent the reachability between the nodes. This graph is calculated during the planning process, includes node verification and edge inspection, can effectively combine environmental information such as obstacles to provide sufficient geometric constraints, and efficiently prune the infeasible action sequences in the continuous domain by a heuristic method, thereby ensuring feasibility in the final complete path trajectory optimization.

[0073] The reachability graph construction module includes two sub-modules, a node sampling module and an edge connection module.

[0074] The node sampling module is responsible for randomly sampling nodes in the task space. These nodes represent the possible positions that the end effector of the robot can reach. This sampling process is evenly and randomly distributed, so it is sufficient to cover the entire task space. The robotic arm model is defined as a local operating system with a floating base and a torso, which is represented by the following formula:

[0075] ;

[0076] Among them, q represents the configuration of the robotic arm, where q base is the position and orientation of the base, is the base node of the coordinates (x, y), is the orientation of the base node coordinates, and q manipulator is the joint angle vector of the manipulator. The term "manipulator" means robotic arm. Among them, q1, q2, q3,..., q m represents the angle vector of each joint of the manipulator.

[0077] The verification process of the node is shown in the following formula:

[0078] ;

[0079] In this formula, x0 is the node to be checked (the target position of the end effector), M is a constant customized according to the robotic arm configuration and is used to handle collision prevention in optimization, and g prec (x0) is a function used to check for collisions between x0 and the environment, and f kin (q) is the mapping function from joint angles to the end effector position, and g(q) is the inequality constraint of the robot joint angles, such as joint angle limits.

[0080] The edge connection module is carried out after node sampling. Its purpose is to establish effective connections for the nodes within the reach of the robotic arm for all the nodes in the graph, so as to generate potential motion paths of the robotic arm. Each edge in the edge connection is assigned a cost value, and this cost value can be based on the distance between nodes, the expected motion time, energy consumption, or other metrics related to the path quality. Its cost formula is shown as follows:

[0081] ;

[0082] In the formula, C e is the cost of the edge, and are respectively the positions of the two endpoints of the edge in the task space, and are the robot configurations (joint angles) corresponding to these two positions, , and are the weight coefficients corresponding to the position, configuration, and collision cost, and are the collision cost values of the nodes. For example, if a node may collide with an obstacle, the cost will increase.

[0083] S3. Based on the constructed reachability graph, in the path query system based on LGP and the reachability graph, it is updated with the feedback during the movement of the robotic arm, and the solutions with changed costs are deleted, and all the paths and intermediate nodes between two nodes, as well as the costs therein, are stored simultaneously.

[0084] See Figure 2 , specifically, in the path query system based on logical geometric planning and the reachability graph, there is a solution database to store the previously calculated solutions, reducing the time for re-planning the same path. The solution library is updated with the feedback during the movement of the robotic arm, and the solutions with changed costs are deleted.

[0085] In the solution library, all the paths and intermediate nodes between two nodes, as well as the costs therein, are stored simultaneously.

[0086] S4. In the path query system based on LGP and the reachability graph, the start and end coordinates of each path are set as the starting point and the ending point. The reachability graph planner receives the start and end coordinates of each path as the starting point and the ending point, and uses Dijkstra (the Dijkstra algorithm) to find the shortest path between the starting point and the ending point in the reachability graph. It verifies whether there is a collision according to the distance of the simulated robotic arm movement. If there is a collision, it is saved to the solution library; after removing the edges with collisions from the reachability graph, re-planning is carried out. Similarly, when the edges connecting nodes are removed, the process of supplementing additional intermediate points is also triggered, which is an iterative process; the optimized path is passed to the control system of the robotic arm for execution. During the execution process, the state of the machine and environmental changes are monitored in real time, and dynamic adjustment is carried out if necessary.

[0087] Specifically, the path planning module is the path query system based on logical geometric planning and the reachability graph. Figure 2 It details the interaction between the reachability graph and the LGP framework in path planning.

[0088] The reachability graph planner receives the start and end coordinates of each path as the starting point and the ending point, named x k and x k+1 . When constructing the reachability graph, the nodes corresponding to x k and x k+1 are added to the reachability graph, and it is checked whether there are obstacles or collisions in the path directly connecting the two nodes. If x k and x k+1 cannot be connected to the existing graph, additional intermediate nodes will be supplemented. Here, Gaussian sampling and covariance are used to find adjacent nodes as the intermediate nodes connecting to the existing graph. During this process, obstacle checks are also carried out on the nodes and edges to ensure the reachability of the intermediate nodes and calculate the costs.

[0089] This process uses Dijkstra to find the shortest path between two points in the reachability graph. Subsequently, based on the distance of simulating the movement of the robotic arm, it verifies whether there are collisions. After removing the edges with collisions from the reachability graph, re-planning is carried out. Similarly, after removing the connected edges between nodes, the process of supplementing additional intermediate points will also be triggered. This is an iterative process. During the planning process, the system updates the graph through enhancement and edge removal to ensure that the path planning is optimal or approximately optimal.

[0090] Finally, the optimized path is passed to the control system of the robotic arm for execution. During the execution process, the status of the machine and environmental changes are monitored in real time, and dynamic adjustments are made if necessary.

[0091] The present invention optimizes high-degree-of-freedom mobile manipulators such as robotic arms, and can effectively handle complex kinematic problems and large-scale state spaces, while some traditional methods often only adapt to single-dimensional planar movement and may face challenges in scalability and computational efficiency.

[0092] By introducing a sampling-based reachability graph, R-LGP (which means Extended Logic Geometric Planning) can incorporate environmental information (such as obstacles) and provide sufficient geometric constraints, which helps to generate feasible action sequences, reduce the risk of collisions, and improve the search efficiency. While previous solutions mostly adopted simplified environmental models, which may lead to re-planning during the movement process.

[0093] Adopting a hierarchical method to optimize the planning and motion trajectory in stages, this structured method helps to solve complex problems and improve the feasibility of planning, while many previous solutions only optimized one of task planning or motion planning for the TAMP problem.

[0094] See Figure 3 a system for extending the motion trajectory of a robotic arm based on LGP, including

[0095] a motion path planning modeling module, which is used to model the motion path planning of the robotic arm found by the RU (rack unit) through data optimization;

[0096] a reachability graph construction module, which is used to construct a reachability graph based on the modeling of the motion path planning. Nodes and edges are provided in the reachability graph. The nodes represent the positions of the end effectors of the mobile manipulator, and the edges represent the reachability between the nodes. By combining node verification and edge inspection with obstacle and existing production environment information, sufficient geometric constraints are provided, and infeasible action sequences in the continuous domain are pruned through heuristic methods, so as to ensure feasibility in the final complete path trajectory optimization. The construction of the reachability graph is completed;

[0097] A path query module, which is used to update in a path query system based on LGP and reachability graph with the feedback during the movement of the robotic arm, delete the solutions with changed costs, and simultaneously store all paths and intermediate nodes between two nodes, as well as the costs therein.

[0098] A path planning module, which is used in a path query system based on LGP and reachability graph. Set the start and end coordinates of each path as the starting point and the ending point. The reachability graph planner receives the start and end coordinates of each path as the starting point and the ending point, uses Dijkstra to find the shortest path between the starting point and the ending point in the reachability graph, and verifies whether there is a collision according to the distance of the simulated robotic arm movement. If there is a collision, it is saved to the solution library; after removing the edges with collisions from the reachability graph, re-plan. Similarly, when the edges connecting nodes are removed, the process of supplementing additional intermediate points will also be triggered, and this is an iterative process; the optimized path is passed to the control system of the robotic arm for execution. During the execution process, the status of the machine and environmental changes are monitored in real time, and dynamic adjustment is performed if necessary.

[0099] Those skilled in the art should understand that the embodiments of the present invention can be provided as a method, a system, or a computer program product. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0100] The present invention is described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to the embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as the combination of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate means for realizing the functions specified in Figure 1 one process or multiple processes and / or blocks Figure 1 one block or multiple blocks.

[0101] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured article including instruction means, and the instruction means realizes the functions specified in Figure 1 one process or multiple processes and / or blocksFigure 1 The functions specified in one or more boxes.

[0102] These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are performed on the computer or other programmable device to generate a computer-implemented process. Thus, the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one Figure 1 process or more processes and / or boxes Figure 1 or more boxes.

[0103] Although the preferred embodiments of the present invention have been described, those skilled in the art can make additional changes and modifications once they learn the basic creative concept. Therefore, the appended claims are intended to be construed as including the preferred embodiments and all changes and modifications falling within the scope of the present invention.

[0104] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention also intends to include these modifications and variations.

Claims

1. A method for extending the motion trajectory of a robotic arm in LGP. In the abstract way of LGP logic geometric programming for task and motion planning problems, it is characterized in that Including, S1, model the motion path planning of the RU search robotic arm through data optimization, where RU is the rack unit; S2, based on the modeling of the motion path planning, construct a reachability graph. The reachability graph has nodes and edges. The nodes represent the positions of the end effectors of the mobile manipulator, and the edges represent the reachability between the nodes. By combining node verification and edge inspection with obstacle and existing production environment information, sufficient geometric constraints are provided. Prune the infeasible action sequences in the continuous domain through heuristic methods, so as to ensure feasibility in the final complete path trajectory optimization. The construction of the reachability graph is completed; S3, based on the constructed reachability graph, in the path query system based on LGP and reachability graph, update with the feedback during the movement of the robotic arm, delete the solutions with changed costs, and simultaneously store all the paths and intermediate nodes between two nodes, as well as the costs therein; S4, in the path query system based on LGP and reachability graph, set the head and tail coordinates of each path as the start and end points. The reachability graph planner receives the head and tail coordinates of each path as the start and end points, and uses Dijkstra to find the shortest path between the start and end points in the reachability graph. Verify whether there is a collision according to the distance of the simulated robotic arm movement. If there is a collision, save it to the solution library; after removing the edges with collisions from the reachability graph, re-plan. Similarly, removing the edges connected between nodes will also trigger the process of supplementing additional intermediate points, which is an iterative process; transfer the optimized path to the control system of the robotic arm for execution. During the execution process, monitor the state of the machine and environmental changes in real time, and make dynamic adjustments if necessary.

2. A method for expanding the motion trajectory of a robotic arm of an LGP according to claim 1, characterized in that In step S1, it also includes, The data optimization method is: The formula is as follows: ； Where x represents a series of objects of the movement path, that is, the nodes in the reachability graph, a represents the constraint conditions, g(x,a) and h(x,a) represent the constraints of x in different dimensions, and K is a coefficient.

3. A method for expanding the motion trajectory of a robotic arm of an LGP according to claim 1, characterized in that, In step S1, it also includes, S11, determine a symbolic task execution order. The first layer of search is symbolic, without directly considering the kinematics and environmental constraints of the robot, and generate a symbolic sequence; S12, convert the symbolic sequence generated by the first layer of the LGP into a series of key frames. By optimizing these key frames, the LGP can consider the kinematic limitations and environmental constraints of the robot while maintaining the task logic order, so as to generate a task execution plan.

4. A method for expanding the motion trajectory of a robotic arm of an LGP according to claim 1, characterized in that, In step S2, for the construction of the reachability graph, it also includes, A node sampling module, which is responsible for randomly sampling nodes in the task space. These nodes represent the possible positions that the end effector of the robot can reach. This sampling process is evenly and randomly distributed, so it is sufficient to cover the entire task space. The robotic arm model is defined as a local operating system with a floating base and a torso, and is represented by the following formula: ； Among them, q represents the configuration of the robotic arm, where q base is the position and orientation of the base, is the base node of the coordinates (x, y), is the orientation of the base node coordinates, q manipulator is the joint angle vector of the manipulator, q1, q2, q3,..., q m represents the angle vector of each joint of the manipulator.

5. A method for expanding the motion trajectory of a robotic arm of an LGP according to claim 4, characterized in that, In step S2, for the construction of the reachability graph, it also includes, Edge connection module, which is performed after node sampling to establish effective connections for the nodes within the reach of the robotic arm, thereby generating potential motion paths of the robotic arm; Each edge in the edge connection is assigned a cost value, which is based on the distance between nodes, the expected motion time, energy consumption, or other metrics related to path quality; Its cost formula is as follows: ； In the formula, Ce is the cost of the edge, x1 and x2 are the positions of the two endpoints of the edge in the task space, q1 and q2 are the robot configurations corresponding to these two positions, w x , w q and w c are the weight coefficients corresponding to the position, configuration, and collision costs, and c1 and c2 are the collision cost values of the nodes.

6. A system for extending the motion trajectory of a robotic arm of LGP, characterized in that, Motion path planning modeling module, which is used to model the motion path planning of the robotic arm by the RU (rack unit) to find the motion trajectory of the robotic arm through data optimization; Reachability graph construction module, which is used to construct a reachability graph based on the modeling of the motion path planning. The reachability graph is provided with nodes and edges. The nodes represent the positions of the end effectors of the mobile manipulator, and the edges represent the reachability between nodes. By combining node verification and edge inspection with obstacle and existing production environment information, sufficient geometric constraints are provided, and infeasible action sequences in the continuous domain are pruned by heuristic methods, so as to ensure feasibility in the final complete path trajectory optimization. After the reachability graph construction is completed; Path query module, which is used to update based on the feedback during the movement of the robotic arm in the path query system based on LGP and the reachability graph, delete the solutions with changed costs, and store all the paths and intermediate nodes between two nodes, as well as the costs therein; Path planning module, which is used in the path query system based on LGP and the reachability graph. Set the head and tail coordinates of each path as the start and end points. The reachability graph planner receives the head and tail coordinates of each path as the start and end points, and uses Dijkstra to find the shortest path between the start and end points in the reachability graph. Verify whether there is a collision according to the distance of the simulated robotic arm movement. If there is a collision, save it to the solution library; After removing the edges with collisions from the reachability graph, re-plan. Similarly, removing the connected edges between nodes will also trigger the process of supplementing additional intermediate points, which is an iterative process; Transmit the optimized path to the control system of the robotic arm for execution. During the execution process, monitor the state of the machine and environmental changes in real time, and make dynamic adjustments if necessary.

7. The system for expanding the motion trajectory of the robotic arm of the LGP according to claim 6, wherein, In the motion path planning modeling module, it also includes, The data optimization method is: The formula is as follows: ； Where x represents a series of objects of the movement path, that is, the nodes in the reachability graph, a represents the constraint conditions, g(x,a) and h(x,a) represent the constraints of x in different dimensions, and K is a coefficient.

8. The system for expanding the motion trajectory of the robotic arm of the LGP according to claim 6, characterized in that, In the motion path planning modeling module, it also includes, Conformant sequence decision layer unit, which is used to determine a symbolic task execution order. The search in the first layer is symbolic and does not directly consider the kinematics and environmental constraints of the robot, and generates a symbolic sequence; Optimized key frame unit, which is used to convert the symbolic sequence generated by the first layer of the LGP into a series of key frames. By optimizing these key frames, LGP can consider the kinematic limitations and environmental constraints of the robot while maintaining the task logic order, thereby generating a task execution plan.

9. The system for expanding the motion trajectory of the robotic arm of the LGP according to claim 6, characterized in that, In the path query module, it also includes, a node sampling module. The node sampling module is responsible for randomly sampling nodes in the task space. These nodes represent the possible positions that the end effector of the robot can reach. This sampling process is evenly and randomly distributed, so it is sufficient to cover the entire task space. The robotic arm model is defined as a local operating system with a floating base and a torso, and is represented by the following formula: ； Among them, q represents the configuration of the robotic arm, and q base is the position and orientation of the base, is the base node of the coordinates (x, y), is the orientation of the base node coordinates, and q manipulator is the joint angle vector of the manipulator, q1, q2, q3,..., q m represents the angle vector of each joint of the manipulator.

10. A system for expanding the robotic arm motion trajectory of an LGP according to claim 6, characterized in that, In the path query module, it also includes, an edge connection module. The edge connection module is performed after node sampling to establish effective connections between the nodes within the reach of the robotic arm, thereby generating potential motion paths of the robotic arm; Each edge in the edge connection is assigned a cost value, which is based on the distance between nodes, the expected motion time, energy consumption, or other metrics related to the path quality; Its cost formula is as follows: ； In the formula, Ce is the cost of the edge, x1 and x2 are the positions of the two endpoints of the edge in the task space, q1 and q2 are the robot configurations corresponding to these two positions, w x , w q and w c are the weight coefficients corresponding to the position, configuration, and collision costs, and c1 and c2 are the collision cost values of the nodes.

Images