Robotic arm trajectory correction device and related computer program product

Abstract

The present invention provides a robot arm operation trajectory correction device and a related computer program. The robot arm operation trajectory correction device obtains the joint space planning data of the robot arm, determines the control sequence of the joints of the robot arm, converts the joint space planning data of the robot arm into Cartesian space planning data; analyzes to obtain initial planning trajectory data; collects the angle data, speed data and acceleration data of the joints during the operation of the robot arm; performs smoothness evaluation, accuracy evaluation and collision risk evaluation on the initial planning trajectory data; according to the smoothness evaluation results, accuracy evaluation results and collision risk evaluation results, combined with the angle data, speed data and acceleration data of the robot arm joints, analyzes to obtain the trajectory optimization data of the joints. Based on the above scheme, the planning trajectory is corrected according to the state of the robot arm feedback to ensure high efficiency, high precision and high safety of the operation process.

Description

Technical Field

[0001] The present invention relates to the field of industrial robot control, and more particularly to a robot arm running trajectory correction device and a related computer program product applied to a 3C product manufacturing production line. Background Art

[0002] Trajectory planning refers to calculating the expected motion trajectory of the robot according to the requirements of the task. Trajectory planning is generally expressed as a function of motion variables such as displacement, velocity, acceleration, etc. with respect to time. In this function, the input is the expected motion trajectory, kinematic and dynamic parameters of the robot, and the output is the motion variables of each joint or end effector of the robot. In order to meet the requirements of actual industrial production, an excellent trajectory planning method should fully consider the joint wear and motion smoothness of the robot.

[0003] In contrast, trajectory correction is to adjust and correct the robot's trajectory based on the control algorithm according to the sensory feedback and the real-time status of the robot to ensure the safety and accuracy of its motion trajectory. It can be said that trajectory correction is a supplement and optimization of trajectory planning in practice. Based on trajectory correction, the robot can complete the given task with higher efficiency and higher accuracy.

[0004] In the prior art, the control process of a robot or a robotic arm is usually one-way, that is, it can only be controlled by inputting control instructions. Therefore, multiple calibrations and environmental configurations are required before controlling the operation of the robot or robotic arm. However, the production line runs at an extremely high speed and sudden conditions may occur at any time, which makes calibration and configuration meaningless. In order to ensure the safety and efficiency of the robot's operation, the industry urgently needs to propose a trajectory correction solution. Summary of the invention

[0005] The technical problem to be solved by the present invention is that, in view of the above-mentioned defects of the prior art, a method, system and storage medium for correcting the running trajectory of a robot arm are provided. In the process of inputting instructions to control the robot arm, the trajectory is corrected according to the state fed back by the robot arm, which has higher efficiency, higher precision and higher safety.

[0006] The technical solution adopted by the present invention to solve the technical problem is as follows:

[0007] A method for correcting the running trajectory of a mechanical arm, the method is based on a mechanical arm running trajectory correction device, the device comprises an angle sensor, a speed sensor, an acceleration sensor, a laser radar and an industrial robot controller; the angle sensor, the speed sensor and the acceleration sensor are arranged on the mechanical arm, the angle sensor is used to collect the angle data of the mechanical arm joint, the speed sensor is used to collect the speed data of the mechanical arm joint, and the acceleration sensor is used to collect the acceleration data of the mechanical arm joint; the laser radar is arranged above or on the side of the mechanical arm, and is used to collect the mechanical arm motion space data; the industrial robot controller is used to analyze and process the angle data, speed data, acceleration data and mechanical arm motion space data, and control the operation of the mechanical arm; the method comprises the following steps:

[0008] S1. Acquire the joint space planning data of the robot arm, and determine the control sequence of the joints of the robot arm according to the joint space planning data;

[0009] S2. According to the control sequence of the joints of the robot arm, the joint space planning data of the robot arm is converted into Cartesian space planning data of the robot arm;

[0010] S3. Based on the Cartesian space planning data of the robot arm, the initial planning trajectory data of the robot arm is obtained, and the initial planning trajectory data describes the planning trajectory of the robot arm in the Cartesian space;

[0011] S4. Real-time acquisition of robot arm joint motion data, acquisition of joint angle data, velocity data and acceleration data during the operation of the robot arm, wherein the angle data indicates the deflection angle of the joint, the velocity data indicates the operating speed of the joint, and the acceleration data indicates the rate of change of the operating speed of the joint;

[0012] S5. Perform a smoothness evaluation on the initial planning trajectory data to obtain a smoothness evaluation result;

[0013] S6. performing accuracy evaluation on the initial planning trajectory data to obtain an accuracy evaluation result;

[0014] S7. Collecting three-dimensional point cloud data of obstacles in the movement space of the robot arm during the operation of the robot arm, combining the initial planning trajectory data and the three-dimensional point cloud data of the obstacles to perform collision risk assessment, and obtain a collision risk assessment result;

[0015] S8. Based on the smoothness evaluation results, accuracy evaluation results and collision risk evaluation results, combined with the angle data, speed data and acceleration data of the robot arm joints, the trajectory optimization data of the joints is obtained through dynamic correction algorithm analysis, and the trajectory optimization data includes angle adjustment data, speed adjustment data and acceleration adjustment data.

[0016] Compared with the existing technology, the beneficial effects of this technical solution are: converting joint space planning data into Cartesian space planning data, collecting angle data, speed data and acceleration data in real time, and performing smoothness, accuracy and collision risk assessments, and correcting the planned trajectory according to the status of the robot arm feedback to ensure high efficiency, high precision and high safety of the operation process.

[0017] Further, in step S1, obtaining the joint space planning data of the robot arm, and determining the control sequence of the joints of the robot arm according to the joint space planning data specifically includes the following steps:

[0018] S101. Obtaining the initial position coordinates of the end effector of the robot arm in Cartesian space as A(x1, y1, z1), and obtaining the target position coordinates of the end effector of the robot arm in Cartesian space as B(x2, y2, z2);

[0019] S102. According to the initial position coordinates A (x1, y1, z1) and the target position coordinates B (x2, y2, z2), the joint angle changes Δθ1, Δθ2, Δθ3 corresponding to the joints of the robot arm from the initial joint angle to the target joint angle are analyzed;

[0020] S103. Calculate a path in the joint space based on a linear interpolation method to control the robot arm to smoothly transition from an initial joint angle to a target joint angle;

[0021] S104. Generate the running trajectory of the joints of the robot arm in Cartesian space through cubic polynomial interpolation to obtain joint trajectory data θi(t):

[0022] θi(t)=a0+a1t+a2t 2 +a3t 3 ;

[0023] Where t is the running time, θi(t) is the angle of joint i at time t, and a0, a1, a2, and a3 are the coefficients of the polynomial;

[0024] S105. Perform parameter analysis based on the initial joint angle, target joint angle and robot arm running time to calculate the corresponding values ​​of a0, a1, a2 and a3;

[0025] S106. Obtain the control sequence of the joint based on a0, a1, a2, a3 and the joint trajectory data θi(t).

[0026] The beneficial effect of adopting the above scheme is that the robotic arm is composed of multiple joints. After determining the corresponding joint angle change in step S1, the joint angle corresponding to a specific time point in the control process can be clearly determined through the joint trajectory data, thereby obtaining the control sequence of the joint, and directly obtaining the control instructions of the robotic arm.

[0027] Further, in step S2, converting the joint space planning data of the robot arm into the Cartesian space planning data of the robot arm specifically includes the following steps:

[0028] S201. Obtain DH matrix parameters of the joint, wherein the DH matrix parameters include the length a of the first link l1 , length of the second connecting rod a l2 , first connecting rod offset d l1 , Second connecting rod offset d l2 , the first joint angle θ l1 , the second joint angle θ l2 , first joint twist and second joint twist

[0029] S202. Establish a forward kinematics model based on the DH matrix parameters and calculate the first transformation matrix A1 (θ l1 ) and the second transformation matrix A2(θ l2 ), the first transformation matrix is ​​used to describe A1(θ l1 )The posture of the end effector of the first link in Cartesian space, the second transformation matrix A2 (θ l2 ) is used to describe the posture of the end effector of the second link in Cartesian space, where:

[0030]

[0031]

[0032] S203. According to the first transformation matrix A1(θ l1 ) and the second transformation matrix A2(θ l2 ) Calculate the overall transformation matrix in,

[0033] S204. According to the overall transformation matrix The joint space planning data of the robot arm is converted into the Cartesian space planning data of the robot arm.

[0034] The beneficial effect of adopting the above scheme is: after obtaining the overall transformation matrix through analysis of the first transformation matrix and the second transformation matrix, the joint space planning data of the robot arm can be converted into the Cartesian space planning data of the robot arm based on the overall transformation matrix, making the robot arm control process more intuitive.

[0035] Further, in step S5, performing a smoothness evaluation on the initial planned trajectory data of the robot arm to obtain a smoothness evaluation result specifically includes the following steps:

[0036] S501. Collect the trajectory data of the robot arm on the two-dimensional plane (x s i ,y s i ) and the corresponding timestamp t s i , where i = 1, 2, ..., N;

[0037] S502. Obtain three consecutive points P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3);

[0038] S503. According to P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3) Calculate the trajectory curvature K i ,in,

[0039]

[0040] S504. According to the trajectory curvature K i Calculate the trajectory curvature change rate ΔK i ,in

[0041]

[0042] S505. According to the trajectory curvature K i and the rate of change of trajectory curvature ΔK i Performing a smoothness assessment to obtain a smoothness assessment result;

[0043] In step S6, the accuracy evaluation of the initial planned trajectory data of the robot arm to obtain the accuracy evaluation result specifically includes the following steps:

[0044] S601. Collect the trajectory data of the robot arm on the two-dimensional plane (x s i ,y s i ) and the corresponding timestamp t s i , where i = 1, 2, ..., N;

[0045] S602. Based on the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i Forming actual running trajectory data;

[0046] S603. Perform accuracy evaluation by comparing the actual running trajectory data with the initial planned trajectory data to obtain an accuracy evaluation result.

[0047] The beneficial effects of adopting the above scheme are: smoothness evaluation is performed based on the trajectory curvature and the rate of change of trajectory curvature. The smaller the values ​​of trajectory curvature and the rate of change of trajectory curvature, the better the smoothness; by comparing the actual running trajectory data with the initial planned trajectory data, the higher the degree of overlap, the better the accuracy. Through the above scheme, the initial planned trajectory data will be quantitatively evaluated, which is conducive to improving the scientific nature of control.

[0048] Further, in step S7, the collision risk assessment is performed in combination with the initial planning trajectory data and the three-dimensional point cloud data of the obstacle, and obtaining the collision risk assessment result specifically includes the following steps:

[0049] S701. Establish an obstacle model in Cartesian space based on the three-dimensional point cloud data of the obstacle;

[0050] S702. Segmenting the initial planned trajectory data into a plurality of continuous planned trajectory segments, wherein the planned trajectory segments include a starting point, an intermediate point and an end point within each trajectory segment;

[0051] S703: Match the planned trajectory segment to the obstacle model, determine whether there is an overlap between the obstacle model and the planned trajectory segment, and then output a collision risk assessment result.

[0052] The beneficial effect of adopting the above scheme is: an obstacle model is established based on the three-dimensional point cloud data of obstacles in the robot arm's operating space, and collision risk assessment is performed by introducing the initial planned trajectory data to determine whether there is any overlapping part between the obstacle model and the planned trajectory fragments. This can eliminate the collision risk during operation in real time and improve safety.

[0053] Correspondingly, a robot arm running trajectory correction system, the system is based on a robot arm running trajectory correction device, the device includes an angle sensor, a speed sensor, an acceleration sensor, a laser radar and an industrial robot controller; the angle sensor, the speed sensor, and the acceleration sensor are arranged on the robot arm, the angle sensor is used to collect the angle data of the robot arm joint, the speed sensor is used to collect the speed data of the robot arm joint, and the acceleration sensor is used to collect the acceleration data of the robot arm joint; the laser radar is arranged above or on the side of the robot arm, and is used to collect the robot arm motion space data; the industrial robot controller is used to analyze and process the angle data, speed data, acceleration data and robot arm motion space data, and control the operation of the robot arm; the system includes:

[0054] A space planning data acquisition module is used to acquire the space planning data of the joints of the robot arm and determine the control sequence of the joints of the robot arm according to the space planning data of the joints;

[0055] A coordinate system data conversion module, used for converting the joint space planning data of the robot arm into the Cartesian space planning data of the robot arm according to the control sequence of the joints of the robot arm;

[0056] A planning trajectory analysis module, used to obtain initial planning trajectory data of the robotic arm based on the Cartesian space planning data of the robotic arm, wherein the initial planning trajectory data describes the planning trajectory of the robotic arm in the Cartesian space;

[0057] A real-time data acquisition module is used to acquire the joint motion data of the robot arm in real time, and to acquire the rotation angle data, velocity data and acceleration data of the joint during the operation of the robot arm. The rotation angle data indicates the deflection angle of the joint, the velocity data indicates the operation speed of the joint, and the acceleration data indicates the rate of change of the operation speed of the joint;

[0058] A smoothness evaluation module is used to perform a smoothness evaluation on the initial planning trajectory data to obtain a smoothness evaluation result;

[0059] An accuracy evaluation module is used to evaluate the accuracy of the initial planning trajectory data and obtain an accuracy evaluation result;

[0060] The collision risk assessment module is used to collect the three-dimensional point cloud data of obstacles in the movement space of the robot arm during the operation of the robot arm, and conduct collision risk assessment by combining the initial planning trajectory data and the three-dimensional point cloud data of the obstacles to obtain the collision risk assessment result;

[0061] The trajectory optimization module is used to obtain the trajectory optimization data of the joints through dynamic correction algorithm analysis based on the smoothness evaluation results, accuracy evaluation results and collision risk evaluation results, combined with the angle data, speed data and acceleration data of the robot arm joints, wherein the trajectory optimization data includes angle adjustment data, speed adjustment data and acceleration adjustment data.

[0062] Furthermore, the space planning data acquisition module includes:

[0063] A coordinate calculation unit, used to obtain the initial position coordinates of the end effector of the robot arm in Cartesian space as A (x1, y1, z1), and obtain the target position coordinates of the end effector of the robot arm in Cartesian space as B (x2, y2, z2);

[0064] An angle calculation unit, used to analyze and obtain the joint angle changes Δθ1, Δθ2, Δθ3 corresponding to the joints of the robotic arm from the initial joint angle to the target joint angle according to the initial position coordinates A (x1, y1, z1) and the target position coordinates B (x2, y2, z2);

[0065] A first trajectory calculation unit is used to calculate a path in the joint space to control the robot arm to smoothly transition from an initial joint angle to a target joint angle based on a linear interpolation method;

[0066] The second trajectory calculation unit is used to generate the running trajectory of the joints of the robot arm in the Cartesian space by cubic polynomial interpolation to obtain the joint trajectory data θi(t):

[0067] θi(t)=a0+a1t+a2t 2 +a3t 3 ;

[0068] Where t is the running time, θi(t) is the angle of joint i at time t, and a0, a1, a2, and a3 are the coefficients of the polynomial;

[0069] A parameter analysis unit is used to perform parameter analysis based on the initial joint angle, the target joint angle and the robot arm running time, and calculate the corresponding values ​​of a0, a1, a2 and a3;

[0070] The control sequence analysis unit is used to obtain the control sequence of the joint based on a0, a1, a2, a3 and the joint trajectory data θi(t).

[0071] Furthermore, the coordinate system data conversion module includes:

[0072] A parameter acquisition unit is used to acquire the DH matrix parameters of the joint, wherein the DH matrix parameters include the length a of the first link l1, length of the second connecting rod a l2 , first connecting rod offset d l1 , Second connecting rod offset d l2 , the first joint angle θ l1 , the second joint angle θ l2 , first joint twist and second joint twist

[0073] The first transformation matrix calculation unit is used to establish a forward kinematics model based on the DH matrix parameters and calculate the first transformation matrix A1 (θ l1 ) and the second transformation matrix A2(θ l2 ), the first transformation matrix is ​​used to describe A1(θ l1 )The posture of the end effector of the first link in Cartesian space, the second transformation matrix A2 (θ l2 ) is used to describe the posture of the end effector of the second link in Cartesian space, where:

[0074]

[0075]

[0076] The second transformation matrix calculation unit calculates the first transformation matrix A1 (θ l1 ) and the second transformation matrix A2(θ l2 ) Calculate the overall transformation matrix in,

[0077] Coordinate system data conversion unit, used to transform the overall matrix The joint space planning data of the robot arm is converted into the Cartesian space planning data of the robot arm.

[0078] Furthermore, the smoothness evaluation module includes:

[0079] The first two-dimensional trajectory acquisition unit is used to acquire the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i , where i = 1, 2, ..., N;

[0080] The two-dimensional coordinate acquisition unit is used to obtain the three continuous points P1 (x s 1,y s 1) P2(x s 2,y s 2) and P3(x s3,y s 3);

[0081] The trajectory curvature calculation unit is used to calculate the trajectory curvature according to P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3) Calculate the trajectory curvature K i ,in,

[0082]

[0083] The trajectory curvature change rate calculation unit is used to calculate the trajectory curvature K i Calculate the trajectory curvature change rate ΔK i ,in

[0084]

[0085] Smoothness evaluation unit, used to calculate the curvature K of the trajectory. i and the rate of change of trajectory curvature ΔK i Performing a smoothness assessment to obtain a smoothness assessment result;

[0086] The accuracy assessment module includes:

[0087] The second two-dimensional trajectory acquisition unit is used to collect the trajectory data of the robot arm on the two-dimensional plane (x s i ,y s i ) and the corresponding timestamp t s i , where i = 1, 2, ..., N;

[0088] The running trajectory calculation unit is used to calculate the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i Forming actual running trajectory data;

[0089] An accuracy evaluation unit, used for performing accuracy evaluation by comparing actual operation trajectory data with initial planned trajectory data to obtain an accuracy evaluation result;

[0090] The collision risk assessment module comprises:

[0091] A model building unit, used for building an obstacle model in Cartesian space according to three-dimensional point cloud data of the obstacle;

[0092] A trajectory segmentation unit, used to segment the initial planned trajectory data into a plurality of continuous planned trajectory segments, wherein the planned trajectory segments include a starting point, an intermediate point and an end point in each trajectory segment;

[0093] The collision risk assessment unit is used to match the planned trajectory segment to the obstacle model, determine whether there is any overlap between the obstacle model and the planned trajectory segment, and then output a collision risk assessment result.

[0094] Correspondingly, a storage medium stores a computer program, wherein the computer program includes program instructions. When the program instructions are executed by a processor, the processor executes the robot arm operation trajectory correction method as described above. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] Figure 1 It is a flow chart of the method for correcting the running trajectory of the robot arm of the present invention.

[0096] Figure 2 It is a structural schematic diagram of the robot arm running trajectory correction system of the present invention.

[0097] In the figure, the components represented by the reference numerals are shown as follows:

[0098] Spatial planning data acquisition module 1, coordinate system data conversion module 2, planning trajectory analysis module 3, real-time data acquisition module 4, smoothness evaluation module 5, accuracy evaluation module 6, collision risk evaluation module 7, trajectory optimization module 8. DETAILED DESCRIPTION

[0099] In order to make the purpose, technical solution and advantages of the present invention clearer and more specific, the present invention is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0100] In the description of the present invention, it should be understood that the orientations or positional relationships indicated by the terms "center", "upper", "lower", "front", "back", "left", "right", etc. are based on the orientations or positional relationships shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or component referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first" and "second" are used for descriptive purposes only and cannot be understood as indicating or implying relative importance.

[0101] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected" and "connected" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be an indirect connection through an intermediate medium, or it can be a connection between the two components. When a component is referred to as being "fixed to" or "disposed on" another element, it can be directly on the other component or there can also be a central component. When a component is considered to be "connected" to another element, it can be directly connected to the other element or there can be a central element at the same time. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0102] Trajectory planning refers to calculating the expected motion trajectory of the robot according to the requirements of the task. Trajectory planning is generally expressed as a function of motion variables such as displacement, velocity, acceleration, etc. with respect to time. In this function, the input is the expected motion trajectory, kinematic and dynamic parameters of the robot, and the output is the motion variables of each joint or end effector of the robot. In order to meet the requirements of actual industrial production, an excellent trajectory planning method should fully consider the joint wear and motion smoothness of the robot.

[0103] In contrast, trajectory correction is to adjust and correct the robot's trajectory based on the control algorithm according to the sensory feedback and the real-time status of the robot to ensure the safety and accuracy of its motion trajectory. It can be said that trajectory correction is a supplement and optimization of trajectory planning in practice. Based on trajectory correction, the robot can complete the given task with higher efficiency and higher accuracy.

[0104] In the prior art, the control process of a robot or a robotic arm is usually one-way, that is, it can only be controlled by inputting control instructions, so multiple calibrations and environmental configurations are required before controlling the robot or robotic arm to operate. However, the production line runs at an extremely high speed and unexpected conditions may occur at any time, which makes calibration and configuration meaningless.

[0105] In order to solve the above technical problems, the technical solution of the present application is based on a robot arm operation trajectory correction device, and proposes a robot arm operation trajectory correction method, system and storage medium.

[0106] A robot arm running trajectory correction device, the device includes an angle sensor, a speed sensor, an acceleration sensor, a laser radar and an industrial robot controller; the angle sensor, the speed sensor and the acceleration sensor are arranged on the robot arm, the angle sensor is used to collect the angle data of the robot arm joint, the speed sensor is used to collect the speed data of the robot arm joint, and the acceleration sensor is used to collect the acceleration data of the robot arm joint; the laser radar is arranged above or on the side of the robot arm, and is used to collect the robot arm motion space data; the industrial robot controller is used to analyze and process the angle data, speed data, acceleration data and robot arm motion space data, and control the operation of the robot arm. It should be noted that the above sensors are equipped for data collection, which is a general application of existing technologies and existing products. This technical solution does not optimize the structure and control method of the above angle sensor, speed sensor, acceleration sensor, laser radar and industrial robot controller, but only uses the data collected by the above components for creative development and utilization.

[0107] like Figure 1 As shown, a method for correcting the trajectory of a robot arm comprises the following steps:

[0108] S1. Obtain the joint space planning data of the robot, and determine the control sequence of the joints of the robot according to the joint space planning data. In the field of robot control, there are usually joint space and Cartesian space. Generally speaking, the joint space describes the operation process of each joint from the perspective of the robot, and the Cartesian space describes the operation process of each joint from the perspective of the user. The former can be directly used for robot control, while the latter is simpler and easier to understand. In step S1, first call the pre-set joint space planning data. The joint space planning data can control the joint to run to a certain angle and a certain position at a certain time. The control sequence of the joints of the robot can be determined by the joint space planning data.

[0109] S2. According to the control sequence of the joints of the robot arm, the joint space planning data of the robot arm is converted into the Cartesian space planning data of the robot arm. The purpose of step S2 is to reflect the planning data of the robot arm in the Cartesian space. Through the Cartesian space planning data, the operator can directly know the whole operation process of the robot arm in the planning and design.

[0110] S3. Analyze the Cartesian space planning data of the robot arm to obtain the initial planning trajectory data of the robot arm, and the initial planning trajectory data describes the planning trajectory of the robot arm in the Cartesian space. The initial planning trajectory data in step S3 can be understood as the planning path of the robot arm in the Cartesian space. The Cartesian space planning data in step S2 is connected in segments to obtain the initial planning trajectory data in the Cartesian space.

[0111] S4. Real-time collection of robot arm joint motion data, collection of joint angle data, speed data and acceleration data during the operation of the robot arm, the angle data represents the deflection angle of the joint, the speed data represents the operating speed of the joint, and the acceleration data represents the rate of change of the operating speed of the joint. Step S4 is one of the innovations of this technical solution. In the prior art, the rotation angle, rotation speed and acceleration of the robot arm joint are mostly controlled by inputting instructions. In this technical solution, in addition to paying attention to "input", more attention is paid to "output". By real-time monitoring of the robot arm, the angle data, speed data and acceleration data of the joints on it are collected, and attention is paid to the state output by the robot arm.

[0112] S5. Perform a smoothness evaluation on the initial planned trajectory data to obtain a smoothness evaluation result. The purpose of step S5 is to perform a smoothness evaluation on the initial planned trajectory data to determine whether the robot arm will have problems such as freezing and steering obstruction during operation according to the planned trajectory.

[0113] S6. Evaluate the accuracy of the initial planned trajectory data to obtain an accuracy evaluation result. The purpose of step S6 is to evaluate the accuracy of the initial planned trajectory data to determine whether the robot arm can operate strictly according to the planned trajectory.

[0114] S7. Collect the three-dimensional point cloud data of obstacles in the robot arm's motion space during the operation of the robot arm, and perform collision risk assessment on the initial planned trajectory data and the three-dimensional point cloud data of the obstacles to obtain a collision risk assessment result. The purpose of step S7 is to perform a collision risk assessment on the initial planned trajectory data to determine whether there is a collision risk when the robot arm is operating according to the planned trajectory.

[0115] S8. According to the smoothness evaluation results, the accuracy evaluation results and the collision risk evaluation results, in combination with the angle data, speed data and acceleration data of the mechanical arm joint, the trajectory optimization data of the joint is obtained through dynamic correction algorithm analysis, and the trajectory optimization data includes angle adjustment data, speed adjustment data and acceleration adjustment data. In step S8, the smoothness evaluation results, the accuracy evaluation results and the collision risk evaluation results dynamically adjust the angle data, speed data and acceleration data of the mechanical arm joint, that is, it can be understood that when any of the smoothness evaluation results, the accuracy evaluation results or the collision risk evaluation results has negative feedback, that is, there is a problem with smoothness and accuracy or there is a collision risk, that is, the angle data, speed data and acceleration data of the mechanical arm joint are dynamically adjusted to obtain angle adjustment data, speed adjustment data and acceleration adjustment data. According to the angle data, speed data and acceleration data and the corresponding angle adjustment data, speed adjustment data and acceleration adjustment data, the operation of the mechanical arm can be optimized.

[0116] Based on the above technical solution, the joint space planning data is converted into Cartesian space planning data, the angle data, speed data and acceleration data are collected in real time, and the smoothness, accuracy and collision risk assessment are performed. The planned trajectory is corrected according to the status of the robot arm feedback. When the planned path becomes unreasonable due to other unpredictable factors, it is corrected in time to ensure high efficiency, high precision and high safety of the operation process.

[0117] Step S1 is described in detail using a three-joint robotic arm. Preferably, in step S1, obtaining the joint space planning data of the robotic arm, and determining the control sequence of the joints of the robotic arm according to the joint space planning data specifically includes the following steps:

[0118] S101. The initial position coordinates of the end effector of the robot arm in Cartesian space are obtained as A(x1, y1, z1), and the target position coordinates of the end effector of the robot arm in Cartesian space are obtained as B(x2, y2, z2). Cartesian space can be understood as an ordinary three-dimensional space. The purpose of step S101 is to obtain the initial position and target position of the end effector of the robot arm, and further determine the coordinates of the two points, wherein the initial position coordinates are A(x1, y1, z1) and the target position coordinates are B(x2, y2, z2).

[0119] S102. According to the initial position coordinates A(x1, y1, z1) and the target position coordinates B(x2, y2, z2), the joint angle changes Δθ1, Δθ2, Δθ3 corresponding to the joints of the robot arm from the initial joint angle to the target joint angle are analyzed. Through step S101, it is known that the initial position coordinates are A(x1, y1, z1) and the target position coordinates are B(x2, y2, z2), thereby determining the initial joint angle and target joint angle of each joint, and then obtaining the joint angle changes Δθ1, Δθ2, Δθ3 corresponding to each joint.

[0120] S103. Based on the linear interpolation method, a path is calculated in the joint space to control the robot arm to smoothly transition from the initial joint angle to the target joint angle. Step S103 uses linear interpolation to plan the path. The calculation amount of linear interpolation is relatively small, and the path planning result can be generated in a shorter time, which can meet the requirements of real-time trajectory optimization during operation in this technical solution.

[0121] S104. Generate the running trajectory of the joints of the robot arm in Cartesian space through cubic polynomial interpolation to obtain joint trajectory data θi(t):

[0122] θi(t)=a0+a1t+a2t 2 +a3t 3;

[0123] Wherein, t is the running time, θi(t) is the angle of joint i at time t, and a0, a1, a2, and a3 are the coefficients of the polynomial. a0, a1, a2, and a3 are all constants, but according to the different mechanical characteristics of different manipulators, the data of a0, a1, a2, and a3 are not the same. In step S104, the running trajectory of the joints of the manipulator in Cartesian space is generated by cubic polynomial interpolation, which can improve the movement accuracy and smoothness of the manipulator, reduce the driving load, simplify the path planning process, and improve the efficiency of the manipulator motion control. The joint trajectory data can reflect the state of each joint at a certain moment, and prepare for the analysis of the control sequence.

[0124] S105. Perform parameter analysis based on the initial joint angle, target joint angle and robot running time, and calculate the corresponding values ​​of a0, a1, a2, and a3. In the joint trajectory data θi(t), t is time, and θi(t) is the angle of joint i at time t. Assuming that the running time is from 0 to 5 seconds, the robot is controlled in seconds. Since the speed and acceleration are both 0 at the start and end times, step S105 sets the starting and end conditions, that is, θi(0) = initial joint angle, θi(5) = target joint angle, to solve the coefficients a0, a1, a2, and a3 of the polynomial, so as to further obtain the joint space planning data to determine the control sequence of the joints of the robot.

[0125] S106. Based on a0, a1, a2, a3 and joint trajectory data θi(t), obtain the control sequence of the joint. Taking the running time from 0 to 5 seconds and the control of the robot arm in seconds as an example, in step S106, the control sequence is:

[0126] t = 0s, θ = initial joint angle;

[0127] t=1s,θ=θ(1)

[0128] t=2s,θ=θ(2)

[0129] t=3s,θ=θ(3)

[0130] t=4s,θ=θ(4)

[0131] t=5s,θ=target joint angle.

[0132] Based on the above technical solution, after determining the corresponding joint angle change, the joint angle corresponding to a specific time point in the control process can be clearly determined through the joint trajectory data, thereby obtaining the joint control sequence, and directly obtaining the control instructions of the robot arm. Obtaining the joint space planning data of the robot arm and determining the control sequence of the joints of the robot arm based on these data can improve the movement accuracy, adaptability and stability of the robot arm, while simplifying the trajectory planning and debugging process.

[0133] To facilitate understanding of the technical solution, the two-joint robotic arm is simplified into a structure of two connecting rods, each connecting rod is connected by a rotating joint, that is, the robotic arm includes a first connecting rod, a second connecting rod, a first joint and a second joint to illustrate the technical solution in detail.

[0134] Preferably, in step S2, converting the joint space planning data of the robot arm into the Cartesian space planning data of the robot arm specifically comprises the following steps:

[0135] S201. Obtain DH matrix parameters of the joint, wherein the DH matrix parameters include the length a of the first link l1 , length of the second connecting rod a l2 , first connecting rod offset d l1 , Second connecting rod offset d l2 , the first joint angle θ l1 , the second joint angle θ l2 , first joint twist and second joint twist In step S201, the length a of the first connecting rod can be obtained through the mechanical parameters of the joint. l1 , length of the second connecting rod a l2 , first connecting rod offset d l1 , Second connecting rod offset d l2 , the first joint angle θ l1 , the second joint angle θ l2 , first joint twist and second joint twist And other parameters.

[0136] S202. Establish a forward kinematics model based on the DH matrix parameters and calculate the first transformation matrix A1 (θ l1 ) and the second transformation matrix A2(θ l2 ), the first transformation matrix is ​​used to describe A1(θ l1 )The posture of the end effector of the first link in Cartesian space, the second transformation matrix A2 (θ l2 ) is used to describe the posture of the end effector of the second link in Cartesian space, where:

[0137]

[0138] In step S202, according to the length a of the first connecting rod l1 , length of the second connecting rod a l2 , first connecting rod offset d l1 , Second connecting rod offset d l2 , the first joint angle θ l1 , the second joint angle θ l2 , first joint twist and second joint twist The same parameters can be analyzed to obtain the first transformation matrix A1(θ l1 ) and the second transformation matrix A2(θ l2 ).

[0139] S203. According to the first transformation matrix A1(θ l1 ) and the second transformation matrix A2(θ l2 ) Calculate the overall transformation matrix in, In step S203, according to the first transformation matrix A1(θ l1 ) and the second transformation matrix A2(θ l2 ) to further calculate the overall transformation matrix

[0140] S204. According to the overall transformation matrix The joint space planning data of the robot arm is converted into the Cartesian space planning data of the robot arm. After step S204, the overall transformation matrix is ​​obtained. After that, the corresponding Cartesian space planning data can be inferred from the known joint space planning data.

[0141] Based on the above technical solution, after the overall transformation matrix is ​​obtained by analyzing the first transformation matrix and the second transformation matrix, the joint space planning data of the robot arm can be converted into the Cartesian space planning data of the robot arm based on the overall transformation matrix, making the robot arm control process more intuitive.

[0142] Preferably, in step S5, performing a smoothness evaluation on the initial planned trajectory data of the robot arm to obtain a smoothness evaluation result specifically includes the following steps:

[0143] S501. Collect the trajectory data of the robot arm on the two-dimensional plane (x s i ,y s i ) and the corresponding timestamp t s i, where i = 1, 2, ..., N. It should be noted that the superscript s in the parameters involved in step S5 is the abbreviation of smoothness, which is intended to distinguish the parameters in other steps and has no other mathematical and physical meanings. The two-dimensional plane described in step S501 can be an XY plane, a YZ plane, or an XZ plane. It only needs to follow the principle of clear trajectory, that is, according to the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i The resulting trajectory is clearly readable on this two-dimensional plane.

[0144] S502. Obtain three consecutive points P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3) The continuous points in step S502 are the three continuous points obtained under the minimum control time gap condition. For example, in the process of implementing the present technical solution, the robot arm is controlled in seconds, and the three points are the positions that are connected in three seconds under the operation according to the preset control instructions.

[0145] S503. According to P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3) Calculate the trajectory curvature K i ,in,

[0146]

[0147] In step S503, based on three consecutive points P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3) The corresponding trajectory curvature K can be calculated i It is understandable that the more points are selected, the more trajectory curvature K can be obtained. i , the data has greater reference significance.

[0148] S504. According to the trajectory curvature K i Calculate the trajectory curvature change rate ΔKi ,in

[0149]

[0150] S505. According to the trajectory curvature K i and the rate of change of trajectory curvature ΔK i A smoothness evaluation is performed to obtain a smoothness evaluation result.

[0151] “Trajectory” is originally a concept in the field of image processing. Based on the above steps, whether it is the trajectory curvature K i , or the rate of change of trajectory curvature ΔK i , can be quantitatively calculated to evaluate its smoothness. Smoothness evaluation is performed based on trajectory curvature and trajectory curvature change rate. The smaller the trajectory curvature and trajectory curvature change rate, the better the smoothness. Through the above scheme, the initial planning trajectory data will be quantitatively evaluated, which is conducive to improving the scientific nature of control.

[0152] In step S6, the accuracy evaluation of the initial planned trajectory data of the robot arm to obtain the accuracy evaluation result specifically includes the following steps:

[0153] S601. Collect the trajectory data of the robot arm on the two-dimensional plane (x s i ,y s i ) and the corresponding timestamp t s i , where i=1, 2, ..., N. Step S601 is the same as step S501. In the specific implementation process, data collection can be performed once, and then smoothness evaluation and accuracy evaluation can be performed in parallel, or they can be performed in steps.

[0154] S602. Based on the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i The purpose of step S602 is to use the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i , and draw the corresponding actual running trajectory data.

[0155] S603. Obtain the accuracy evaluation result by comparing the actual operation trajectory data and the initial planning trajectory data. When implementing step S603, the actual operation trajectory data and the initial planning trajectory data can be moved to the same plane for comparison, or other comparison methods can be used to evaluate the accuracy. By comparing the actual operation trajectory data and the initial planning trajectory data, the higher the degree of overlap, the better the accuracy. Through the above scheme, the initial planning trajectory data will be quantitatively evaluated, which is conducive to improving the scientific nature of control.

[0156] Preferably, in step S7, the collision risk assessment is performed in combination with the initial planning trajectory data and the three-dimensional point cloud data of the obstacle, and obtaining the collision risk assessment result specifically includes the following steps:

[0157] S701. Establish an obstacle model in Cartesian space according to the three-dimensional point cloud data of the obstacle. In step S701, the three-dimensional point cloud data of the obstacle is obtained by scanning the robot arm operation space in real time, based on which the obstacle model can be established in Cartesian space.

[0158] S702. Segment the initial planned trajectory data into a plurality of continuous planned trajectory segments, wherein the planned trajectory segments include a starting point, an intermediate point, and an end point in each trajectory segment. In step 702, the initial planned trajectory data is segmented and refined to obtain a plurality of continuous planned trajectory segments, and then the starting point, the intermediate point, and the end point in each trajectory segment are further determined, thereby maximally determining the path of the robot arm.

[0159] S703. Match the planned trajectory segment to the obstacle model, determine whether the obstacle model and the planned trajectory segment have overlapping parts, and then output the collision risk assessment result. In step S703, if the obstacle model and the planned trajectory segment have overlapping parts, it means that there is a collision risk; if the obstacle model and the planned trajectory segment do not have overlapping parts, it means that there is no collision risk.

[0160] Based on the above technical solution, an obstacle model is established according to the three-dimensional point cloud data of obstacles in the robot arm's operating space. The collision risk is assessed by introducing the initial planned trajectory data to determine whether there is any overlapping between the obstacle model and the planned trajectory fragments. This can eliminate the collision risk during operation in real time and improve safety.

[0161] like Figure 2 As shown, a robot arm running trajectory correction system includes a space planning data acquisition module 1, a coordinate system data conversion module 2, a planning trajectory analysis module 3, a real-time data acquisition module 4, a smoothness evaluation module 5, an accuracy evaluation module 6, a collision risk evaluation module 7, and a trajectory optimization module 8. Specifically:

[0162] The space planning data acquisition module 1 is used to acquire the space planning data of the joints of the robot arm and determine the control sequence of the joints of the robot arm according to the space planning data of the joints;

[0163] A coordinate system data conversion module 2, used for converting the joint space planning data of the robotic arm into the Cartesian space planning data of the robotic arm according to the control sequence of the joints of the robotic arm;

[0164] A planning trajectory analysis module 3 is used to obtain initial planning trajectory data of the robotic arm based on the Cartesian space planning data of the robotic arm, wherein the initial planning trajectory data describes the planning trajectory of the robotic arm in the Cartesian space;

[0165] The real-time data acquisition module 4 is used to acquire the joint motion data of the robot arm in real time, and to acquire the rotation angle data, velocity data and acceleration data of the joint during the operation of the robot arm, wherein the rotation angle data indicates the deflection angle of the joint, the velocity data indicates the operation speed of the joint, and the acceleration data indicates the rate of change of the operation speed of the joint;

[0166] A smoothness evaluation module 5 is used to perform a smoothness evaluation on the initial planning trajectory data to obtain a smoothness evaluation result;

[0167] The accuracy evaluation module 6 is used to perform accuracy evaluation on the initial planning trajectory data to obtain an accuracy evaluation result;

[0168] The collision risk assessment module 7 is used to collect the three-dimensional point cloud data of obstacles in the movement space of the robot arm during the operation of the robot arm, and to perform collision risk assessment in combination with the initial planning trajectory data and the three-dimensional point cloud data of the obstacles to obtain a collision risk assessment result;

[0169] The trajectory optimization module 8 is used to obtain the trajectory optimization data of the joint through dynamic correction algorithm analysis based on the smoothness evaluation results, the accuracy evaluation results and the collision risk evaluation results, combined with the angle data, speed data and acceleration data of the robot arm joint, wherein the trajectory optimization data includes angle adjustment data, speed adjustment data and acceleration adjustment data.

[0170] Preferably, the space planning data acquisition module 1 includes:

[0171] A coordinate calculation unit, used to obtain the initial position coordinates of the end effector of the robot arm in Cartesian space as A (x1, y1, z1), and obtain the target position coordinates of the end effector of the robot arm in Cartesian space as B (x2, y2, z2);

[0172] An angle calculation unit, used to analyze and obtain the joint angle changes Δθ1, Δθ2, Δθ3 corresponding to the joints of the robotic arm from the initial joint angle to the target joint angle according to the initial position coordinates A (x1, y1, z1) and the target position coordinates B (x2, y2, z2);

[0173] A first trajectory calculation unit is used to calculate a path in the joint space to control the robot arm to smoothly transition from an initial joint angle to a target joint angle based on a linear interpolation method;

[0174] The second trajectory calculation unit is used to generate the running trajectory of the joints of the robot arm in the Cartesian space by cubic polynomial interpolation to obtain the joint trajectory data θi(t):

[0175] θi(t)=a0+a1t+a2t 2 +a3t 3 ;

[0176] Where t is the running time, θi(t) is the angle of joint i at time t, and a0, a1, a2, and a3 are the coefficients of the polynomial;

[0177] A parameter analysis unit is used to perform parameter analysis based on the initial joint angle, the target joint angle and the robot arm running time, and calculate the corresponding values ​​of a0, a1, a2 and a3;

[0178] The control sequence analysis unit is used to obtain the control sequence of the joint based on a0, a1, a2, a3 and the joint trajectory data θi(t).

[0179] Preferably, the coordinate system data conversion module 2 includes:

[0180] A parameter acquisition unit is used to acquire the DH matrix parameters of the joint, wherein the DH matrix parameters include the length a of the first link l1 , length of the second connecting rod a l2 , first connecting rod offset d l1 , Second connecting rod offset d l2 , the first joint angle θ l1 , the second joint angle θ l2 , first joint twist and second joint twist

[0181] The first transformation matrix calculation unit is used to establish a forward kinematics model based on the DH matrix parameters and calculate the first transformation matrix A1 (θ l1 ) and the second transformation matrix A2(θ l2 ), the first transformation matrix is ​​used to describe A1(θ l1)The posture of the end effector of the first link in Cartesian space, the second transformation matrix A2 (θ l2 ) is used to describe the posture of the end effector of the second link in Cartesian space, where:

[0182]

[0183]

[0184] The second transformation matrix calculation unit calculates the first transformation matrix A1 (θ l1 ) and the second transformation matrix A2(θ l2 ) Calculate the overall transformation matrix in,

[0185] Coordinate system data conversion unit, used to transform the overall matrix The joint space planning data of the robot arm is converted into the Cartesian space planning data of the robot arm.

[0186] Preferably, the smoothness evaluation module 5 comprises:

[0187] The first two-dimensional trajectory acquisition unit is used to acquire the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i , where i = 1, 2, ..., N;

[0188] The two-dimensional coordinate acquisition unit is used to obtain the three continuous points P1 (x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3);

[0189] The trajectory curvature calculation unit is used to calculate the trajectory curvature according to P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3) Calculate the trajectory curvature K i ,in,

[0190]

[0191] The trajectory curvature change rate calculation unit is used to calculate the trajectory curvature K iCalculate the trajectory curvature change rate ΔK i ,in

[0192]

[0193] Smoothness evaluation unit, used to calculate the curvature K of the trajectory. i and the rate of change of trajectory curvature ΔK i A smoothness evaluation is performed to obtain a smoothness evaluation result.

[0194] Preferably, the accuracy assessment module 6 comprises:

[0195] The second two-dimensional trajectory acquisition unit is used to collect the trajectory data of the robot arm on the two-dimensional plane (x s i ,y s i ) and the corresponding timestamp t s i , where i = 1, 2, ..., N;

[0196] The running trajectory calculation unit is used to calculate the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i Forming actual running trajectory data;

[0197] The accuracy evaluation unit is used to perform accuracy evaluation by comparing the actual operation trajectory data with the initial planned trajectory data to obtain an accuracy evaluation result.

[0198] Preferably, the collision risk assessment module 7 comprises:

[0199] A model building unit, used for building an obstacle model in Cartesian space according to three-dimensional point cloud data of the obstacle;

[0200] A trajectory segmentation unit, used to segment the initial planned trajectory data into a plurality of continuous planned trajectory segments, wherein the planned trajectory segments include a starting point, an intermediate point and an end point in each trajectory segment;

[0201] The collision risk assessment unit is used to match the planned trajectory segment to the obstacle model, determine whether there is any overlap between the obstacle model and the planned trajectory segment, and then output a collision risk assessment result.

[0202] Correspondingly, the technical solution also includes a storage medium, which stores a computer program. The computer program includes program instructions. When the program instructions are executed by a processor, the processor executes the robot arm operation trajectory correction method as described above.

[0203] It should be understood that the application of the present invention is not limited to the above examples. For ordinary technicians in this field, improvements or changes can be made based on the above description. All these improvements and changes should fall within the scope of protection of the claims attached to the present invention.

Claims

1. A robot arm trajectory correction device, characterized in that: The device includes an angle sensor, a speed sensor, an acceleration sensor, a laser radar and an industrial robot controller; the angle sensor, the speed sensor and the acceleration sensor are arranged on a mechanical arm, the angle sensor is used to collect angle data of the mechanical arm joint, the speed sensor is used to collect speed data of the mechanical arm joint, and the acceleration sensor is used to collect acceleration data of the mechanical arm joint; the laser radar is arranged above or on the side of the mechanical arm to collect the mechanical arm motion space data; the industrial robot controller is used to analyze and process the angle data, speed data, acceleration data and mechanical arm motion space data, and control the operation of the mechanical arm; The method performed by the device includes the following steps: S1. Acquire the joint space planning data of the robot arm, and determine the control sequence of the joints of the robot arm according to the joint space planning data; S2. According to the control sequence of the joints of the robot arm, the joint space planning data of the robot arm is converted into Cartesian space planning data of the robot arm; S3. Based on the Cartesian space planning data of the robot arm, the initial planning trajectory data of the robot arm is obtained, and the initial planning trajectory data describes the planning trajectory of the robot arm in the Cartesian space; S4. Real-time acquisition of robot arm joint motion data, acquisition of joint angle data, velocity data and acceleration data during the operation of the robot arm, wherein the angle data indicates the deflection angle of the joint, the velocity data indicates the operating speed of the joint, and the acceleration data indicates the rate of change of the operating speed of the joint; S5. Perform a smoothness evaluation on the initial planning trajectory data to obtain a smoothness evaluation result; S6. performing accuracy evaluation on the initial planning trajectory data to obtain an accuracy evaluation result; S7. Collecting three-dimensional point cloud data of obstacles in the movement space of the robot arm during the operation of the robot arm, combining the initial planning trajectory data and the three-dimensional point cloud data of the obstacles to perform collision risk assessment, and obtain a collision risk assessment result; S8. According to the smoothness evaluation results, the accuracy evaluation results and the collision risk evaluation results, combined with the angle data, speed data and acceleration data of the robot arm joint, the trajectory optimization data of the joint is obtained by dynamic correction algorithm analysis, and the trajectory optimization data includes angle adjustment data, speed adjustment data and acceleration adjustment data; Among them, in step S5, performing smoothness evaluation on the initial planned trajectory data of the robot arm to obtain a smoothness evaluation result specifically includes the following steps: S501. Collect the trajectory data of the robot arm on the two-dimensional plane (x s i ,y s i ) and the corresponding timestamp t s i , where i = 1, 2, ..., N; S502. Obtain three consecutive points P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3); S503. According to P1(x s 1,y s 1) P2(x s 2,y s 2) and P3(x s 3,y s 3) Calculate the trajectory curvature K i ,in, S504. According to the trajectory curvature K i Calculate the trajectory curvature change rate ΔK i ,in S505. According to the trajectory curvature K i and the rate of change of trajectory curvature ΔK i Performing a smoothness assessment to obtain a smoothness assessment result; In step S6, the accuracy evaluation of the initial planned trajectory data of the robot arm to obtain the accuracy evaluation result specifically includes the following steps: S601. Collect the trajectory data of the robot arm on the two-dimensional plane (x s i ,y s i ) and the corresponding timestamp t s i , where i = 1, 2, ..., N; S602. Based on the trajectory data (x s i ,y s i ) and the corresponding timestamp t s i Forming actual running trajectory data; S603. Perform accuracy evaluation by comparing the actual running trajectory data with the initial planned trajectory data to obtain an accuracy evaluation result.

2. The device according to claim 1, characterized in that In step S1, the joint space planning data of the robot arm is obtained, and the control sequence of the joints of the robot arm is determined according to the joint space planning data, which specifically includes the following steps: S101. Obtaining the initial position coordinates of the end effector of the robot arm in Cartesian space as A(x1, y1, z1), and obtaining the target position coordinates of the end effector of the robot arm in Cartesian space as B(x2, y2, z2); S102. According to the initial position coordinates A (x1, y1, z1) and the target position coordinates B (x2, y2, z2), the joint angle changes Δθ1, Δθ2, Δθ3 corresponding to the joints of the robot arm from the initial joint angle to the target joint angle are analyzed; S103. Calculate a path in the joint space based on a linear interpolation method to control the robot arm to smoothly transition from an initial joint angle to a target joint angle; S104. Generate the running trajectory of the joints of the robot arm in Cartesian space through cubic polynomial interpolation to obtain joint trajectory data θi(t): θi(t)=a0+a1t+a2t 2 +a3t 3 ; Where t is the running time, θi(t) is the angle of joint i at time t, and a0, a1, a2, and a3 are the coefficients of the polynomial; S105. Perform parameter analysis based on the initial joint angle, target joint angle and robot arm running time to calculate the corresponding values ​​of a0, a1, a2 and a3; S106. Obtain the control sequence of the joint based on a0, a1, a2, a3 and the joint trajectory data θi(t).

3. The device according to claim 1 or 2, characterized in that: In step S2, converting the joint space planning data of the robot arm into the Cartesian space planning data of the robot arm specifically includes the following steps: S201. Obtain DH matrix parameters of the joint, wherein the DH matrix parameters include the length a of the first link l1 , length of the second connecting rod a l2 , first connecting rod offset d l1 , Second connecting rod offset d l2 , the first joint angle θ l1 , the second joint angle θ l2 , first joint twist and second joint twist S202. Establish a forward kinematics model based on the DH matrix parameters and calculate the first transformation matrix A1 (θ l1 ) and the second transformation matrix A2(θ l2 ), the first transformation matrix is ​​used to describe A1(θ l1 )The posture of the end effector of the first link in Cartesian space, the second transformation matrix A2 (θ l2 ) is used to describe the posture of the end effector of the second link in Cartesian space, where: S203. According to the first transformation matrix A1(θ l1 ) and the second transformation matrix A2(θ l2 ) Calculate the overall transformation matrix in, S204. According to the overall transformation matrix The joint space planning data of the robot arm is converted into the Cartesian space planning data of the robot arm.

4. The device according to any one of claims 1 to 2, characterized in that In step S7, the collision risk assessment is performed in combination with the initial planning trajectory data and the three-dimensional point cloud data of the obstacle, and obtaining the collision risk assessment result specifically includes the following steps: S701. Establish an obstacle model in Cartesian space based on the three-dimensional point cloud data of the obstacle; S702. Segmenting the initial planned trajectory data into a plurality of continuous planned trajectory segments, wherein the planned trajectory segments include a starting point, an intermediate point and an end point within each trajectory segment; S703: Match the planned trajectory segment to the obstacle model, determine whether there is an overlap between the obstacle model and the planned trajectory segment, and then output a collision risk assessment result.

5. A computer program product, characterized in that The computer program includes program instructions. When the program instructions are executed by a processor, the processor executes the method performed by the robot arm trajectory correction device according to any one of claims 1 to 4.

Images