Path planning method and system for robot calibration task

Abstract

The invention relates to the technical field of industrial robot motion control and calibration, in particular to a path planning method and system for a robot calibration task, and the method comprises the steps: setting parameters of a laser tracker and a target ball in the calibration task, building a robot kinematics model according to robot DH parameters, on the basis, defining a dynamically updated task coordinate system and a task constraint vector; in a robot joint space, a random sampling path planning framework of an FR algorithm is adopted for planning, and feasible path nodes which meet task constraints and are free of collision are iteratively generated; and S2, performing joint space data key point extraction on the original path generated in the step S2 through an RDP algorithm, and generating a calibration instruction sequence suitable for robot joint space PTP motion. The method has environmental adaptability, practicability and high efficiency, can effectively solve the interference problems of complex joint limiting and work space obstacles (including the ground and workpieces), and remarkably improves the calibration task preparation efficiency.

Description

Technical Field

[0001] This invention relates to the field of industrial robot motion control and calibration technology, specifically to a method and system for generating collision-free, signal-smoothing, and smooth path planning for automated robot calibration tasks under the constraints of external measuring equipment (laser tracker). Background Technology

[0002] The absolute positioning accuracy of industrial robots is crucial for ensuring their operational quality. To improve accuracy, high-precision external measuring equipment such as laser trackers is typically used to calibrate the robot's parameters. During this process, the target ball mounted on the robot's end flange must remain continuously aligned with the laser emitter of the laser tracker throughout the entire movement to maintain an uninterrupted laser path; this requirement is known as the "no-loss" constraint. Essentially, this is a motion planning problem that must satisfy specific end-effector posture constraints under complex joint limitations, environmental obstacles, and other constraints.

[0003] Traditional calibration path generation primarily relies on Cartesian space constraints of calibration points and manual operator adjustments to the robot's posture. This method is inefficient, requires a high level of operator experience, and struggles to guarantee path optimization and safety, especially in complex scenarios such as large robot configuration changes or the installation of extended tools. A common approach is to use point-to-point commands in the robot controller's joint space to move calibration points. However, simple linear motion in joint space cannot guarantee that the end effector posture meets the complex geometric constraints of "continuous alignment with the laser," easily leading to laser loss, interrupting the calibration process, and resulting in low reliability and automation.

[0004] Some existing general motion planning algorithms, such as Rapidly Exploring RandomTree and its variants, can solve obstacle avoidance problems in high-dimensional spaces, but they usually deal with position space or simple pose constraints and are not deeply coupled with complex, dynamically updated task constraints such as "the end-point vector points to a fixed point".

[0005] Chinese invention patent CN202211605825.7 discloses an RRT-connect algorithm that integrates process knowledge, which realizes rapid path planning for industrial robots for specific processes. Although this method improves the efficiency of path search and planning quality by introducing expert knowledge, its expert mechanism depends on pre-set process rules. This method fails to guarantee the satisfaction of calibration task constraints from the planning source.

[0006] Therefore, there is an urgent need for a calibration path planning method and system that can automatically and reliably generate calibration paths that meet the "no light loss" constraint, avoid collisions, and have smooth motion, so as to improve the efficiency, success rate, and applicability of automated robot calibration. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention proposes a path planning method and system for robot calibration tasks.

[0008] The technical problem to be solved by this invention is achieved by the following technical solution: A path planning method for robot calibration tasks includes the following steps: Step S1: Set the parameters of the laser tracker and the target ball in the calibration task, establish the robot kinematic model based on the robot's DH parameters, and define a dynamically updated task coordinate system based on this. and task constraint vector ; Step S2: In the robot joint space, the random sampling path planning framework of the FR algorithm is used for planning, and feasible paths that meet the task constraints and are collision-free are generated iteratively. Step S3: Extract key points of joint space data from the path generated in step S2 using the RDP algorithm to generate a calibration command sequence suitable for robot joint space PTP motion.

[0009] As a further improvement of the present invention, the parameters set in step S1 include: the fixed position of the laser tracker in the robot's base coordinate system. The fixed offset of the target ball center in the robot end flange coordinate system Euler angles of the target sphere mirror in the robot end flange coordinate system .

[0010] As a further improvement of the present invention, the task coordinate system in step S1 The definition is as follows: The origin of the task coordinate system is the position of the target ball's center in the robot's base coordinate system. The X and Y axes of the task coordinate system are defined according to the right-hand rule and their adaptation to the current direction of motion; the Z axis of the task coordinate system is defined from... point to A one-way vector.

[0011] As a further improvement of the present invention, the task constraint vector in step S1 Defined as .

[0012] As a further improvement of the present invention, the planning process of the FR algorithm in step S2 is as follows: Step S21: Initialize the two trees and , The root is , The root is ; Step S22: Enter the main loop: Random sampling Assuming the current activity tree is Find its nearest neighbor ; Step S23, from Towards directional expansion step size, to obtain ; Step S24: Call the FR algorithm to calculate the feasible nodes that satisfy the constraints. ; Step S25: Perform collision detection. If arrive The process of movement and If there is no collision, then add it using the RRT*-Connec algorithm. ; Step S26: Switch the active tree to Try to start from Mid-range The nearest node extends towards it until it hits an obstacle and reaches its destination. Or it may reach the maximum number of steps that can be extended; Step S27: If two trees are successfully connected through some expansion, the planning is successful, and a path from... arrive The initial path.

[0013] As a further improvement to the present invention, the specific process of step S24 is as follows: Step S241, Calculate the node Corresponding task coordinate system and the current position of the target ball at the end. ; Step S242: Calculate the error between the end-effector's pose in the task coordinate system under the current configuration and the desired pose defined by the task constraint vector. ; Step S243: Based on the task-constrained Jacobian matrix pseudo-reversal Calculate joint adjustment amount And perform iterative corrections; Step S244, iterate to Or, by reaching the maximum number of iterations, a feasible node satisfying the constraints can be obtained. ,in This represents the convergence threshold of the FR algorithm.

[0014] As a further improvement of the present invention, in step S25, the collision detection is based on the predefined geometric model of the robot body, the end effector and the environmental obstacles to make an interference judgment.

[0015] As a further improvement of the present invention, key points are extracted in step S3 using the RDP algorithm, and the specific process is as follows: Step S31: Calculate the first difference of the original path point sequence to obtain the Euclidean distance between adjacent path points, and determine the simplified threshold of the RDP algorithm based on the Euclidean distance; Step S32: For a given path segment, calculate the perpendicular distance from all internal points to the chord connecting the beginning and end points of the segment. If the maximum perpendicular distance exceeds the simplification threshold, use the farthest point as the dividing point to divide the path segment into two sub-segments and process them recursively. If it does not exceed the threshold, discard all internal points in the segment and only keep the beginning and end points.

[0016] A path planning system for robot calibration tasks, used to execute the path planning method for robot calibration tasks described above.

[0017] The beneficial effects of this invention are: 1. Environmental Adaptability: The RRT*-Connect framework used in this invention possesses probabilistic completeness and asymptotic optimality. Combined with collision detection, it can effectively handle complex joint constraints and workspace obstacle (including ground and workpiece) interference problems. This invention specifically clarifies its support for calibration scenarios with large robot configuration changes and solves the path safety problem in such scenarios through collision modeling. 2. Practicality and Efficiency: This invention uses the RDP algorithm to intelligently thin the path, reducing the number of path points while maintaining accuracy. This facilitates the generation of concise and efficient robot PTP instruction programs, greatly simplifying engineering applications. The entire planning process is fully automated, eliminating the need for operators to manually adjust lost calibration points and significantly improving the efficiency of calibration task preparation. Attached Figure Description

[0018] The present invention will be further described below with reference to the accompanying drawings and embodiments: Figure 1 This is a system structure block diagram of the present invention; Figure 2 This is a schematic diagram illustrating the definition of the parameterized task coordinate system in this invention; Figure 3 This is a flowchart of the node constraint correction process in the FR algorithm of this invention; Figure 4 This is a schematic diagram of the RRT*-Connect algorithm combined with constraint correction for dual-tree search in this invention; Figure 5 This is a schematic diagram illustrating the principle of using the RDP algorithm to thin out data in the joint space path in this invention. Figure 6 To directly use PTP joint movement commands from Exercise The trajectory diagram showing the change in the angle between the target ball and the laser at that time; Figure 7 This is a diagram showing the trajectory of the target ball and the laser angle after the planning of this invention; Figure 8 This is a simulation result diagram of continuous path planning for multiple calibration points according to the present invention. Detailed Implementation

[0019] To make the technical means, creative features, objectives and effects of this invention easier to understand, the invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0020] A path planning method for robot calibration tasks includes steps S1, S2, and S3. The path planning is performed using a path planning system for robot calibration tasks, such as... Figure 1 As shown, the path planning system includes a task constraint modeling module, a constraint-aware motion planning module, and a path post-processing and robot instruction generation module. The task constraint modeling module executes step S1; the constraint-aware motion planning module executes step S2; and the path post-processing and robot instruction generation module executes step S3.

[0021] The task constraint modeling module is connected to the constraint-aware motion planning module. The task constraint modeling module outputs the established robot kinematic model, defined constraint vectors, and task coordinate system to the constraint-aware motion planning module. The constraint-aware motion planning module is connected to the path post-processing and robot instruction generation module. The constraint-aware motion planning module plans the robot kinematic model, constraint vectors, and task coordinate system established by the task constraint modeling module using a random sampling path planning framework based on the FR algorithm. Iteratively, it generates feasible path nodes that satisfy the task constraints and are collision-free, and sends them to the path post-processing and robot instruction generation module. The path post-processing and robot instruction generation module extracts key points using the RDP algorithm based on the feasible path nodes planned by the constraint-aware motion planning module, generates a calibration instruction sequence suitable for robot joint space PTP motion, and then generates a robot motion program for calibration based on the calibration instruction sequence.

[0022] The specific steps of the path planning method are as follows: Step S1: Set the parameters of the laser tracker and the target ball in the calibration task: the fixed position of the laser tracker in the robot's base coordinate system. The fixed offset of the target ball center in the robot end flange coordinate system Euler angles of the target sphere mirror in the robot end flange coordinate system Among them, fixed offset The settings are based on the installation of the target ball in the calibration task. For any robot configuration... A kinematic model of the robot is established based on the robot's DH parameters, and the position of the target ball's center in the robot's base coordinate system is calculated. Based on this, a dynamically updated task coordinate system is defined. and task constraint vector .like Figure 2 As shown, the task coordinate system The definition is as follows: The origin of the task coordinate system is [omitted]; the X and Y axes of the task coordinate system are defined according to the right-hand rule and their adaptation to the current direction of motion; the Z axis of the task coordinate system is [omitted]. point to A unidirectional vector, thus fully defining the configuration Dynamically updated task coordinate system .

[0023] Finally, the task constraint vector Defined as Its physical meaning is: in the mission coordinate system Under this constraint, the target sphere is allowed to translate along the Z-axis of the mission coordinate system and rotate around the Z-axis (i.e., the direction of the laser vector), but rotation around the X and Y axes is prohibited. This constraint ensures that the normal vector of the target sphere's reflecting surface is always collinear with the Z-axis, i.e., always pointing towards the laser emitting device of the laser tracker.

[0024] Step S2: In the robot joint space, the stochastic sampling path planning framework of the FR algorithm is used for planning, iteratively generating feasible path nodes that satisfy the task constraints and are collision-free. The step size is set to the RRT step size, rrt_step, the target bias probability is goal_bias, the maximum number of RRT iterations is max_planning_iterations, and the convergence threshold of the FR algorithm is... .

[0025] The planning process of the FR algorithm is as follows: Step S21: Define tree nodes as joint angle vectors Initialize two trees and , The root node is , The root node is .

[0026] Step S22: Enter the main loop: Random sampling node Assuming the current activity tree is Find its nearest neighbor node ; Step S23, from node To the node Expand the step size in the direction to obtain the node. .

[0027] Step S24: Call the FR algorithm to calculate the feasible nodes that satisfy the constraints. .like Figure 3 As shown. Specifically: Calculate the homogeneous transformation rotation matrix of the target sphere relative to the mission coordinate system. This yields the error vector in the task space. The error vector includes translation errors along the X, Y, and Z axes of the mission coordinate system. And attitude errors around the X, Y, and Z axes of the mission coordinate system. The attitude error is a rotation matrix. Euler angle vectors. Based on the task constraint vectors. Extract the components that need to be constrained (rotation and translation about the X and Y axes) and calculate the task constraint error. .

[0028] Calculate the Jacobian in the base coordinate system using the vector product method. The Jacobian ratio in the task coordinate system is obtained using the Jacobian transformation formula. ,in, The rotation matrix from the robot's task coordinate system to the base coordinate system is given by the defined task constraint vector. Select The corresponding row is constrained by Jacobi. By constraining Jacobi's pseudo-inverse ,calculate ,in, The step size scaling factor represents the constraint task error in Cartesian space. Convert to robot joint space error In the middle, update the candidate nodes. Repeat this iteration until... To obtain feasible nodes that satisfy the constraints. .

[0029] Step S25: Perform collision detection. If the node... To feasible nodes The motion process and feasible nodes If there is no collision, then the RRT*-Connec algorithm is used to identify feasible nodes. join in Find the node in the neighborhood that minimizes the sum of the root node cost and the cost of moving to that node, and designate it as the parent node. For any two nodes... , The cost is defined as the joint space path length. It attempts to reconnect to neighboring nodes to reduce their costs, and updates the slave node after reconnection. Move to a feasible node The total cost. The collision detection is based on a predefined geometric model of the robot body, the end effector, and environmental obstacles to determine interference. This invention is applicable to calibration scenarios where tools are installed on the robot's end effector; in this step, a collision model including the tool needs to be established.

[0030] Step S26: Switch the active tree to Try to start from Mid-distance feasible nodes The nearest node extends towards it until it encounters an obstacle and reaches a feasible node. Or it may reach the maximum number of steps that can be extended.

[0031] Step S27: If two trees are successfully connected through some expansion, the planning is successful, and a path from node S27 is obtained by backtracking. To the node The initial path, such as Figure 4 The diagram illustrates the process of expanding and connecting two trees.

[0032] Step S3: Analyze the path generated in step S2. The RDP algorithm is used to extract key points from joint space data, generating a calibration command sequence suitable for PTP motion in robot joint space. For example... Figure 5 As shown, the details are as follows: The path obtained in step S2 The RDP algorithm is used to extend its application to six-dimensional joint space. It is the standard mathematical symbol for the set of real numbers, representing the set of all real numbers; Indicates by A matrix space consisting of 6-dimensional real vectors; Represents a dense path point matrix Include There are 10 path points, each corresponding to a set of real joint parameters in the six-dimensional joint space of the industrial robot.

[0033] Considering that robot calibration tasks typically employ point-to-point (PTP) motion commands in joint space to execute movement between points, the motion planning is essentially linear interpolation in joint space, i.e., starting from the initial joint configuration... To the target joint configuration The motion process is governed by a linear relationship , Description, original path The path simplification problem can be transformed into a piecewise linear approximation of the curve in the high-dimensional joint space, where the path is a discrete polyline trajectory with subtle fluctuations. By extracting the key inflection points of the polyline as intermediate points of the path, data compression can be achieved while preserving the global geometric features of the original trajectory.

[0034] This invention identifies key turning points in the original path using the RDP algorithm. The specific implementation steps are as follows: First, calculate the original path point sequence. First-order difference Then, the Euclidean distance between adjacent path points can be obtained. Based on this distance sequence, a simplified threshold for the RDP algorithm is adaptively determined. ,in This is the arithmetic mean of the Euclidean distances between adjacent points. This is an adjustable simplification factor; The larger the value, the higher the degree of simplification, and the more the key points retained can represent the main geometric features of the trajectory.

[0035] The RDP algorithm recursively performs the following operations: For a given path segment, calculate the perpendicular distance from all internal points to the chord connecting the beginning and end points of the segment; if the maximum perpendicular distance exceeds a threshold... If the furthest point is selected as the dividing point, the path segment is divided into two sub-segments and processed recursively; otherwise, all internal points within the segment are discarded, and only the first and last points are retained. Through this process, redundant path points can be significantly eliminated while maintaining the overall geometric features of the path, thus facilitating the subsequent generation of compact and efficient robot-executable instruction sequences.

[0036] Furthermore, the present invention will be further illustrated through specific embodiments.

[0037] Path planning for calibration between two points of a serial robot: This embodiment uses a six-axis industrial robot as the implementation object. The laser tracker is fixed 4 meters in front of the robot, and the target ball is installed at the center of the end flange with an offset of [0,0,0.05] meters. The mirror surface of the target ball is parallel to the surface of the robot flange.

[0038] Set starting point With the target point Define the task coordinate system: the origin is the current position of the target ball, and the Z-axis always points to the laser tracker. Constraint vector. This restricts the end effector's rotation and translation around the X and Y axes of the mission coordinate system, ensuring that the target sphere remains aligned with the laser.

[0039] The stochastic sampling path planning framework of the FR algorithm is used for planning. Initialization is performed using... and Two trees are rooted at each node. After each candidate node is expanded, the Frontier Algorithm (FR) is immediately invoked to calculate the attitude error of that node in the task coordinate system. By constraining the Jacobian matrix pseudo-reversal Calculate joint adjustment amount The process iteratively corrects the error until it meets the threshold. After collision detection, the corrected node is added to the tree according to the RRT* rule (preferential parent node selection, reconnection optimization). The two trees attempt greedily connections; if successful, a preliminary path is generated.

[0040] The generated dense paths are thinned using the RDP algorithm. The perpendicular distances between path points and chords are calculated in six-dimensional joint space, retaining key points with perpendicular distances exceeding a threshold, reducing the number of path points from approximately 150 to 5. Finally, the PTP joint motion commands of the key points are used to generate the robot calibration program.

[0041] like Figure 6 As shown, this demonstrates the direct use of PTP joint motion commands from a point without the planning of the present invention. Exercise The angle between the target ball and the laser changes over time. It is evident that the target ball's direction deviates significantly from the laser tracker during the movement. For example... Figure 7 As shown, the result after planning according to the present invention is demonstrated, with the target ball's direction remaining stably oriented towards the laser tracker throughout the entire end trajectory. Figure 8 The diagram shows the simulation results of continuous path planning for multiple calibration points using the present invention. In summary, the present invention, through constraint modeling and planning framework, achieves fully automated and highly reliable planning of robot calibration paths, effectively solving the pain points of traditional methods and possessing industrial application value.

[0042] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely prisms of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A path planning method for robot calibration tasks, characterized in that: Includes the following steps: Step S1: Set the parameters of the laser tracker and the target ball in the calibration task, establish the robot kinematic model based on the robot's DH parameters, and define a dynamically updated task coordinate system based on this. and task constraint vector ; Step S2: In the robot joint space, the random sampling path planning framework of the FR algorithm is used for planning, and feasible path nodes that meet the task constraints and are collision-free are generated iteratively. Step S3: Extract key points of joint space data from the path generated in step S2 using the RDP algorithm to generate a calibration command sequence suitable for robot joint space PTP motion.

2. The path planning method for robot calibration tasks according to claim 1, characterized in that: The parameters set in step S1 include: the fixed position of the laser tracker in the robot's base coordinate system. The fixed offset of the target ball center in the robot end flange coordinate system Euler angles of the target sphere mirror in the robot end flange coordinate system .

3. The path planning method for robot calibration tasks according to claim 2, characterized in that: Task coordinate system in step S1 The definition is as follows: The origin of the task coordinate system is the position of the target ball's center in the robot's base coordinate system. The X and Y axes of the task coordinate system are defined according to the right-hand rule and their adaptation to the current direction of motion; the Z axis of the task coordinate system is defined from... point to A one-way vector.

4. The path planning method for robot calibration tasks according to claim 3, characterized in that: Task constraint vector in step S1 Defined as .

5. A path planning method for robot calibration tasks according to claim 1, characterized in that: The planning process of the FR algorithm in step S2 is as follows: Step S21: Define tree nodes as joint angle vectors Initialize two trees and , The root node is , The root node is ; Step S22: Enter the main loop: Random sampling node Assuming the current activity tree is Find its nearest neighbor node ; Step S23, from node To the node Expand the step size in the direction to obtain the node. ; Step S24: Call the FR algorithm to calculate the feasible nodes that satisfy the constraints. ; Step S25: Perform collision detection. If the node... To feasible nodes The motion process and feasible nodes If there is no collision, then the RRT*-Connec algorithm is used to identify feasible nodes. join in ; Step S26: Switch the active tree to Try to start from Mid-distance feasible nodes The nearest node extends towards it until it encounters an obstacle and reaches a feasible node. Or it may reach the maximum number of steps that can be extended; Step S27: If two trees are successfully connected through some expansion, the planning is successful, and a path from node S27 is obtained by backtracking. To the node The initial path.

6. A path planning method for robot calibration tasks according to claim 5, characterized in that: The specific process for step S24 is as follows: Step S241, Calculate the node Corresponding task coordinate system and the current position of the target ball at the end. ; Step S242: Calculate the error between the end-effector's pose in the task coordinate system under the current configuration and the desired pose defined by the task constraint vector. ; Step S243: Based on the task-constrained Jacobian matrix pseudo-reversal Calculate joint adjustment amount And perform iterative corrections; Step S244, iterate to Or, by reaching the maximum number of iterations, a feasible node satisfying the constraints can be obtained. ,in This represents the convergence threshold of the FR algorithm.

7. The path planning method for robot calibration tasks according to claim 1, characterized in that: In step S25, collision detection is performed based on a predefined geometric model of the robot body, end effector, and environmental obstacles to determine interference.

8. A path planning method for robot calibration tasks according to claim 1, characterized in that: The specific process in step S3 is as follows: Step S31: Calculate the first difference of the original path point sequence to obtain the Euclidean distance between adjacent path points, and determine the simplified threshold of the RDP algorithm based on the Euclidean distance; Step S32: For a given path segment, calculate the perpendicular distance from all internal points to the chord connecting the beginning and end points of the segment. If the maximum perpendicular distance exceeds the simplification threshold, use the farthest point as the dividing point to divide the path segment into two sub-segments and process them recursively. If it does not exceed the threshold, discard all internal points in the segment and only keep the beginning and end points.

9. A path planning system for robot calibration tasks, characterized in that: A path planning method for performing a robot calibration task as described in any one of claims 1 to 8.

Images