Self-adaptive trajectory planning mechanical arm system and method

Abstract

The invention relates to a self-adaptive trajectory planning mechanical arm system and method, and relates to the technical field of visual inspection automation. The system comprises a mechanical arm, a camera, an LCD display screen and a control unit. The method comprises the steps that firstly, an LCD display screen is controlled to display phase fringe images, feature points are collected through a camera and extracted based on a phase unwrapping algorithm, and a reference transformation matrix from a display screen feature point coordinate system to a mechanical arm base coordinate system is established through hand-eye calibration; when the mounting position of the mechanical arm deviates, the process is executed again to obtain a current transformation matrix, and a correction matrix between the two transformation matrixes is calculated; and finally, all pre-stored motion track points of the mechanical arm are automatically updated by utilizing the correction matrix, so that the motion track points adapt to a new position relation. According to the system, full-automatic and high-precision correction after the track of the mechanical arm deviates is achieved, manual intervention is not needed, the adaptability and stability of the system after migration, copy installation or slight deviation are remarkably improved, and meanwhile the maintenance cost is reduced.

Description

Technical Field

[0001] This invention relates to the field of visual inspection automation technology, and in particular to a robotic arm system and method for adaptive trajectory planning. Background Technology

[0002] As a core piece of equipment in automated production and intelligent material handling, the accuracy of the robotic arm's motion trajectory directly affects the performance of its actions. In practical applications, after reinstallation, relocation of the workspace, or prolonged use, the robotic arm system is prone to positional shifts relative to the worktable, causing the preset motion trajectory to deviate from the target and resulting in problems such as grasping failure and placement deviation.

[0003] To solve the aforementioned offset problem, existing technologies mainly employ the following three solutions:

[0004] 1. Manual calibration: After each installation or detection of deviation, the trajectory parameters are manually adjusted to adapt the trajectory to the current state of the robotic arm. However, this method requires frequent manual intervention, resulting in high labor and time costs. Furthermore, changes in the external environment can affect the stability of the calibration, making it difficult to guarantee trajectory accuracy.

[0005] 2. Visual Guidance: This method relies entirely on 3D vision algorithms for real-time path planning, without using fixed trajectories. This approach requires real-time processing of surrounding 3D environmental information, resulting in high algorithm complexity and long execution time, severely impacting efficiency in industrial production. Furthermore, the algorithm's accuracy is limited by imaging equipment, and its stability is far lower than fixed-trajectory solutions, failing to meet the repetitive operation requirements of mass production in industry.

[0006] 3. Calibration Board Calibration: By fixing the calibration board, images of the calibration board are captured and feature points are extracted. The trajectory is adjusted based on the changes in the extrinsic parameters of the feature points in the camera coordinate system. This method relies on the imaging quality of the calibration board. When the imaging quality is poor, feature point extraction is prone to failure. Furthermore, the calibration board has high requirements for environmental contrast and image coverage, and its anti-interference ability is weak. At the same time, the calibration board can affect the aesthetics in high-end scenarios such as display platforms and commercial applications.

[0007] The existing technologies mentioned above all have their own limitations, making it difficult to achieve efficient and low-cost offset correction while ensuring accuracy and stability. Summary of the Invention

[0008] The technical problem to be solved by the present invention is to provide a robotic arm system and method with adaptive trajectory planning, so as to solve the problem of trajectory failure caused by position deviation of the robotic arm, realize fast and automatic trajectory correction, and improve the adaptability, stability and replicability of the system.

[0009] The technical solution adopted by the present invention to solve its technical problem is: an adaptive trajectory planning robotic arm system, including a robotic arm, a control unit, a camera and an LCD display screen;

[0010] The camera is fixedly mounted at the end of the robotic arm;

[0011] The LCD display screen is fixedly installed in the working area of ​​the robotic arm and is used to display calibration images;

[0012] The control unit is connected to the robotic arm, camera, and LCD display screen respectively;

[0013] The control unit performs the following process:

[0014] S1: Reference calibration process: control the LCD screen to display multiple phase stripe images, acquire images through the camera, and extract multiple feature points from the acquired images based on the phase unwrapping algorithm; based on the feature points, establish and store the reference transformation matrix from the LCD screen feature point coordinate system to the robot arm base coordinate system through hand-eye calibration.

[0015] S2: Trajectory correction process: After detecting a change in the relative position between the robotic arm and the worktable, the reference calibration process is re-executed to obtain the current transformation matrix; the correction matrix between the current transformation matrix and the reference transformation matrix is ​​calculated; the correction matrix is ​​used to transform the pre-stored robotic arm motion trajectory to generate the corrected motion trajectory.

[0016] Furthermore, the phase unwrapping algorithm described in this invention is either the Gray code phase unwrapping method or the three-frequency heterodyne phase unwrapping method.

[0017] Furthermore, before executing the aforementioned benchmark calibration process, the control unit of this invention also executes a field-of-view localization process:

[0018] Control the LCD display to display a positioning pattern with a specific outline;

[0019] Control the end effector of the robotic arm to move to a pre-stored reference pose;

[0020] The camera captures images, and it is determined whether the positioning pattern appears completely in the image.

[0021] If not, the robotic arm end effector is controlled to move within a preset spatial posture range and the judgment is repeated until the positioning pattern appears completely in the image or the maximum number of attempts is reached, at which point an alarm is triggered.

[0022] Furthermore, the preset attitude range described in this invention is set within a range of ±5 degrees based on the Euler angles of the reference pose.

[0023] Furthermore, the feature point extraction described in this invention includes:

[0024] Obtain unwrapped phase maps in the horizontal and vertical directions;

[0025] Pixels that meet the following conditions are selected as feature points in the phase map:

[0026] The horizontal phase value is: m_x + i*(P_X - 2*m_x) / n_x,

[0027] The vertical phase value is: m_y + j*(P_Y - 2*m_y) / n_y.

[0028] Where P_X and P_Y are the phase coordinates on the unwrapped phase maps in the x and y directions, respectively;

[0029] n_x and n_y are the number of feature points taken in the x and y directions, respectively, and there are a total of n_x*n_y feature points;

[0030] i = 1, 2, 3, ..., n_x;

[0031] j = 1, 2, 3, ..., n_y;

[0032] m_x and m_y are the edge spacings in the x and y directions, respectively, in radians.

[0033] To elaborate further, in this invention, n_x=11, n_y=8, m_x=4π, my_y=4π.

[0034] Furthermore, the correction matrix described in this invention includes a correction rotation matrix R_d and a correction translation matrix T_d, calculated as follows:

[0035] ,

[0036] ;

[0037] Where R_p_b and T_p_b are the reference rotation matrix and reference translation matrix, R_p_b_2 and T_p_b_2 are the current rotation matrix and current translation matrix, the superscript T indicates matrix transpose, and × indicates matrix multiplication.

[0038] Furthermore, the specific method by which the control unit of the present invention performs trajectory correction is as follows:

[0039] Let the rotation matrix of the original trajectory target pose be R_t and the translation matrix be T_t. The corrected rotation matrix is ​​R_coor = R_d × R_t, and the corrected translation matrix is ​​T_coor = R_d × T_t + T_d. Convert R_coor to Euler angles, and together with the coordinates corresponding to T_coor, they constitute the corrected target pose.

[0040] Meanwhile, this invention also provides an adaptive trajectory planning method applied to a robotic arm system, characterized by comprising the following steps:

[0041] System setup steps: Fix an LCD display screen in the working area of ​​the robotic arm, and fix a camera at the end of the robotic arm;

[0042] Reference calibration steps: Control the LCD screen to display a preset stripe image, acquire the image through the camera and extract feature points based on the phase unwrapping algorithm; Based on the feature points, establish the initial transformation relationship from the LCD screen feature point coordinate system to the robot arm base coordinate system through hand-eye calibration, and store it as a reference transformation matrix;

[0043] Trajectory planning steps: Plan the initial motion trajectory of the robotic arm according to the task;

[0044] Adaptive correction steps: After detecting a positional offset of the robotic arm, the reference calibration step is re-executed to obtain the current transformation matrix; the correction matrix between the current transformation matrix and the reference transformation matrix is ​​calculated; the initial motion trajectory is corrected using the correction matrix to generate the corrected motion trajectory.

[0045] Furthermore, prior to the benchmark calibration step, the present invention also includes a field-of-view search step: controlling the LCD display to show a specific pattern and moving the end effector of the robotic arm to a predetermined position; determining whether the specific pattern is in the center area of ​​the field of view using the camera; if not, controlling the end effector of the robotic arm to move within a preset angle range to search for the specific pattern.

[0046] The beneficial effect of this invention is that it solves the defects existing in the prior art.

[0047] 1. Compared with manual calibration, the present invention is an automatic calibration that does not require human intervention, saving labor costs and improving efficiency. Moreover, the present invention uses an LCD display screen combined with a phase calibration method, which has strong anti-interference capabilities and solves the problem of poor stability caused by external environmental interference.

[0048] 2. Compared to visual guidance, this invention is a calibration and modification method based on a fixed trajectory. After executing the calibration algorithm once, the effect can be maintained for a longer period of time without frequent algorithm execution. Moreover, the algorithm has low complexity and greatly improved execution efficiency. This invention has lower requirements for imaging equipment, is less affected by the limitations of imaging equipment and external environmental interference, and has strong stability. In addition, this invention can also fine-tune a fixed trajectory based on visual guidance, but unlike fully using visual guidance to plan motion trajectories, it does not frequently call and execute the visual guidance algorithm, which would result in high computational requirements and low execution efficiency.

[0049] 3. Compared to calibration with a calibration board, the calibration hardware used in this invention is an LCD display screen. Compared to the feature points of the calibration board, this invention uses phase calibration, which greatly improves both accuracy and resistance to interference from the external environment. In high-end scenarios (such as display platforms, commercial application scenarios, etc.), placing a calibration board in the application scenario affects the aesthetics, while the display screen used in this invention can also be used for playback and display functions, which is more natural and aesthetically pleasing than the calibration board.

[0050] 4. The overall system has strong compatibility, and the camera can be either 2D or 3D to adapt to different application scenarios; the calibration process is highly automated and easy to operate, which can effectively improve the stability, replicability and safety of the robotic arm system. Attached Figure Description

[0051] Figure 1 This is a simplified hardware structure diagram of the present invention;

[0052] Figure 2 This is a flowchart of the method of the present invention;

[0053] In the picture: 1. Robotic arm; 2. Camera; 3. Workbench; 4. LCD display screen. Detailed Implementation

[0054] The present invention will now be described in further detail with reference to the accompanying drawings and preferred embodiments. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0055] like Figure 1 The illustrated adaptive trajectory planning robotic arm system includes a robotic arm 1, a camera 2, an LCD display 3, a worktable 4, and a control unit. The camera 2 is fixed to the end flange of the robotic arm 1 via a mounting bracket and moves with the robotic arm. The LCD display 3 is a rectangular flat panel display, fixedly mounted on one side of the worktable 4, remaining relatively stationary compared to other fixed components on the worktable. The control unit can be an industrial computer or a PLC, connected to the robotic arm controller, camera, and LCD display via communication lines.

[0056] The overall process of the system is as follows Figure 2 As shown; the specific implementation steps are as follows:

[0057] I. System Setup

[0058] Hardware system architecture such as Figure 1 As shown:

[0059] 1. Robotic arm

[0060] The main motion mechanism is used to complete a series of actions such as grasping and placing.

[0061] 2. Camera

[0062] Fixed to the robotic arm, it can be a 3D camera used for visual guidance, or a regular 2D camera for applications that do not require visual guidance;

[0063] 3. Workbench

[0064] The working area of ​​the robotic arm, including the various targets of the robotic arm's movements, such as the items being gripped, material trays, conveyor belts, and placement platforms, all belong to the workbench section.

[0065] 4. LCD display screen

[0066] The reference object used for calibration should be stationary relative to some fixed, stationary equipment or objects on the workbench. Note that curved displays should not be used; use a rectangular flat display. Measure the dimensions of the display area, width W and height H.

[0067] II. Benchmark Calibration

[0068] 1. Feature point acquisition

[0069] The display screen plays stripe images in the horizontal (x-direction) and vertical (y-direction) directions. The playback order and content are determined by a specific phase unwrapping method (such as three-frequency heterodyne, Gray code, etc.). This invention uses the Gray code phase unwrapping method.

[0070] After unpacking, two phase images are obtained: a horizontal phase image (X) and a vertical phase image (Y). A feature point is selected, with coordinates (x_f, y_f). This feature point satisfies the following conditions (at most one feature point satisfies both of these conditions):

[0071] (1) The phase value of X at (x_f, y_f) is m_x + i*(P_X - 2*m_x) / n_x

[0072] (2) The phase value of Y at (x_f, y_f) is m_y+j*(P_Y-2*m_y) / n_y;

[0073] in:

[0074] P_X and P_Y are the phase coordinates on the unwrapped phase maps in the x and y directions, respectively;

[0075] n_x and n_y are the number of feature points taken in the x and y directions, respectively. There are a total of n_x*n_y feature points. In this invention, n_x=11 and n_y=8.

[0076] i = 1, 2, 3, ..., n_x;

[0077] j = 1, 2, 3, ..., n_y;

[0078] m_x and m_y are the edge spacings in the x and y directions, respectively, in radians. In this invention, m_x = 4π and m_y = 4π.

[0079] The physical coordinates of these points are as follows:

[0080] , where px and py are the phase values ​​of the current feature point on the unwrapped result map in the x and y directions, respectively;

[0081] The above steps yield a set of n_x*n_y feature points;

[0082] 2. Reference Matrix Acquisition

[0083] Using the eye-on-hand calibration method, 10-20 sets of feature points are obtained. Using the Zhang Zhengyou calibration method, the camera intrinsic parameters and distortion parameters, as well as the extrinsic parameters of these feature point sets, are calibrated. At the same time, the end-effector pose of the robotic arm is recorded when these feature point sets are obtained.

[0084] Then, by using these external parameters and the pose of the corresponding end effector of the robotic arm, the rotation moment and translation matrix R_c_e and T_c_e from the camera coordinate system to the end effector coordinate system of the robotic arm are calculated (this belongs to the robotic arm eye calibration on the hand, which is a relatively mature technology).

[0085] Select a set of feature points and obtain the rotation and translation matrices of the feature point set relative to the robot arm's base coordinate system:

[0086] R_p_b= R_e_b x (R_c_e x R_p_c);

[0087] T_p_b=R_e_b x (R_c_e x T_p_c + T_c_e)+T_e_b

[0088] in:

[0089] R_e_b and T_e_b are the rotation and translation matrices from the end effector of the robotic arm to the base coordinate system of the robotic arm, which can be obtained through the robotic arm's own software system;

[0090] R_p_c,T_p_c are a set of rotation and translation matrices from the feature point coordinate system to the camera coordinate system, which can be obtained through the extrinsic parameters of the feature points.

[0091] The symbol x represents matrix multiplication;

[0092] And record the pose of the robotic arm's end effector when taking pictures of this set of feature points, and record it as the reference pose;

[0093] The above steps yield the reference rotation and translation matrices R_p_b and T_p_b;

[0094] III. Trajectory Planning

[0095] Using teaching, coordinate calculation, and other methods, and combining practical application scenarios and needs, a good motion trajectory should be designed;

[0096] IV. Calibration and calibration

[0097] When relocating or replicating the installation at the application site, or when the robotic arm shifts relative to the worktable, the original trajectory will also shift, causing deviations in the movement. Correction is required, and the correction steps are as follows:

[0098] 1. Field of view search

[0099] A specific pattern is displayed on the display screen. This invention uses the company's logo and displays it in full screen size, that is, the pattern size is adjusted to fit the screen and is displayed completely. The screen range is the smallest bounding rectangle of the pattern. The robotic arm is moved to the reference pose and the pattern is template matched in the camera image on the robotic arm. The template matching in this invention uses the NCC algorithm.

[0100] If a match is successful, it means the display screen is within the field of view, and subsequent steps can be performed. If a match fails, the robotic arm is controlled to search within a certain pose range. The search range of this invention is based on a reference pose, with Euler angles within ±5 degrees.

[0101] If a match is found during the search, subsequent actions are performed. If no match is found after the search ends, it indicates that there is an abnormality such as a large offset or a display problem. An abnormality is reported directly to remind manual inspection.

[0102] 2. Obtaining the transformation matrix

[0103] The transformation matrix includes the rotation and translation matrices R_p_b_2, T_p_b_2 from the current feature point coordinate system to the current robot arm base coordinate system;

[0104] Following the method in step two, determine the coordinates of the feature points and calculate R_p_b_2, T_p_b_2;

[0105] 3. Calculation of the correction matrix

[0106]

[0107]

[0108] (R_p_b, T_p_b, R_p_b_2, T_p_b_2, these all represent a variable, but they contain more letters and have similar meanings to variables R and T).

[0109] Where the superscript T denotes transpose, and R_d and T_d represent the correction rotation matrix and correction translation matrix, respectively;

[0110] 4. Trajectory Correction

[0111] Let R_t and T_t be the rotation and translation matrices corresponding to the pose of a target at the end effector of the robotic arm in the trajectory. Then the correction formula for that point is:

[0112] R_coor=R_d x R_t

[0113] T_coor=R_d x T_t+T_d

[0114] R_coor and T_coor are the corrected rotation and translation matrices. T_coor is a 3x1 matrix, and its three elements are the updated coordinates (x_coor, y_coor, z_coor). R_coor is a 3x3 rotation matrix, which is converted to Euler angles (rx_coor, ry_coor, rz_coor). (The conversion of rotation matrix to Euler angles is a fixed and generally accepted formula, which can be written out if needed.) Therefore, the corrected target pose is (x_coor, y_coor, z_coor, rx_coor, ry_coor, rz_coor).

[0115] The corrected pose of each target point in the robotic arm's motion trajectory is calculated using the above method, thereby planning a new trajectory that remains unchanged from the trajectory relative to the worktable before the offset.

[0116] The above description is only a specific embodiment of the present invention. Various examples and illustrations do not constitute a limitation on the substantive content of the present invention. Those skilled in the art can make modifications or variations to the above-described specific embodiments after reading the specification without departing from the substance and scope of the invention.

Claims

1. A robotic arm system with adaptive trajectory planning, characterized in that, Includes a robotic arm, control unit, camera, and LCD display; The camera is fixedly mounted at the end of the robotic arm; The LCD display screen is fixedly installed in the working area of ​​the robotic arm and is used to display calibration images; The control unit is connected to the robotic arm, camera, and LCD display screen respectively; The control unit performs the following process: S1: Reference calibration process: control the LCD screen to display multiple phase stripe images, acquire images through the camera, and extract multiple feature points from the acquired images based on the phase unwrapping algorithm; based on the feature points, establish and store the reference transformation matrix from the LCD screen feature point coordinate system to the robot arm base coordinate system through hand-eye calibration. S2: Trajectory correction process: After detecting a change in the relative position between the robotic arm and the worktable, the reference calibration process is re-executed to obtain the current transformation matrix; the correction matrix between the current transformation matrix and the reference transformation matrix is ​​calculated; the correction matrix is ​​used to transform the pre-stored robotic arm motion trajectory to generate the corrected motion trajectory.

2. The robotic arm system with adaptive trajectory planning according to claim 1, characterized in that, The phase unwrapping algorithm is either Gray code phase unwrapping or three-frequency heterodyne phase unwrapping.

3. The adaptive trajectory planning robotic arm system according to claim 1, characterized in that, Before executing the benchmark calibration procedure, the control unit also executes the field-of-view localization procedure: Control the LCD display to show a positioning pattern with a specific outline; Control the end effector of the robotic arm to move to a pre-stored reference pose; The camera captures images, and it is determined whether the positioning pattern appears completely in the image. If not, the robotic arm end effector is controlled to move within a preset spatial posture range and the judgment is repeated until the positioning pattern appears completely in the image or the maximum number of attempts is reached, at which point an alarm is triggered.

4. The adaptive trajectory planning robotic arm system according to claim 3, characterized in that, The preset attitude range is set within ±5 degrees based on the Euler angles of the reference pose.

5. The robotic arm system with adaptive trajectory planning according to claim 1, characterized in that, The extraction of the feature points includes: Obtain unwrapped phase maps in the horizontal and vertical directions; Pixels that satisfy the following conditions are selected as feature points in the phase map: The horizontal phase value is: m_x + i*(P_X - 2*m_x) / n_x, The vertical phase value is: m_y + j*(P_Y - 2*m_y) / n_y. Where P_X and P_Y are the phase coordinates on the unwrapped phase maps in the x and y directions, respectively; n_x and n_y are the number of feature points taken in the x and y directions, respectively, and there are a total of n_x*n_y feature points; i = 1, 2, 3, ..., n_x; j = 1, 2, 3, ..., n_y; m_x and m_y are the edge spacings in the x and y directions, respectively, in radians.

6. The adaptive trajectory planning robotic arm system according to claim 5, characterized in that, n_x=11, n_y=8, m_x=4π, m_y=4π.

7. The adaptive trajectory planning robotic arm system according to claim 1, characterized in that, The correction matrix includes a correction rotation matrix R_d and a correction translation matrix T_d, and is calculated as follows: ， ； Where R_p_b and T_p_b are the reference rotation matrix and reference translation matrix, R_p_b_2 and T_p_b_2 are the current rotation matrix and current translation matrix, the superscript T indicates matrix transpose, and × indicates matrix multiplication.

8. The robotic arm system with adaptive trajectory planning according to claim 7, characterized in that, The specific method by which the control unit performs trajectory correction is as follows: Let the rotation matrix of the original trajectory target pose be R_t and the translation matrix be T_t. The corrected rotation matrix is ​​R_coor = R_d × R_t, and the corrected translation matrix is ​​T_coor = R_d × T_t + T_d. Convert R_coor to Euler angles, and together with the coordinates corresponding to T_coor, they constitute the corrected target pose.

9. An adaptive trajectory planning method applied to a robotic arm system, characterized in that, Includes the following steps: System setup steps: Fix an LCD display screen in the working area of ​​the robotic arm, and fix a camera at the end of the robotic arm; Reference calibration steps: Control the LCD screen to display a preset stripe image, acquire the image through the camera and extract feature points based on the phase unwrapping algorithm; Based on the feature points, establish the initial transformation relationship from the LCD screen feature point coordinate system to the robot arm base coordinate system through hand-eye calibration, and store it as a reference transformation matrix; Trajectory planning steps: Plan the initial motion trajectory of the robotic arm according to the task; Adaptive correction step: After detecting a positional offset of the robotic arm, the reference calibration step is re-executed to obtain the current transformation matrix; Calculate the correction matrix between the current transformation matrix and the reference transformation matrix; The initial motion trajectory is corrected using the correction matrix to generate the corrected motion trajectory.

10. The adaptive trajectory planning method according to claim 9, characterized in that, Before the benchmark calibration step, a field-of-view search step is also included: controlling the LCD display to display a specific pattern and moving the end effector of the robotic arm to a predetermined position; determining whether the specific pattern is in the center area of ​​the field of view through the camera; if not, controlling the end effector of the robotic arm to move within a preset angle range to search for the specific pattern.

Images