A motion control method for a six-axis robotic arm suitable for hemispherical interior spaces

Abstract

This invention belongs to the field of industrial robot motion control, specifically relating to a motion control method for a six-axis robotic arm suitable for hemispherical internal spaces. Step 1: Implement a point-to-point online motion algorithm; Step 2: Implement robot motion. The beneficial effects are: 1) Based on the special RRRPRR configuration of the six-axis robotic arm, a dual-mode spatial system motion mathematical model of Cartesian and spherical coordinates is constructed, greatly facilitating the conversion between absolute and relative coordinates and providing an intuitive method for quantitative analysis of positioning accuracy; 2) A forward and inverse kinematic model of the robot is established according to the conventional Cartesian coordinate system motion modeling method, and then a model expression under a unified spherical coordinate system is introduced using spherical coordinate transformation solutions, serving as the mathematical algorithm basis for real-time operator control; 3) After obtaining a motion control algorithm within a quarter-sphere, another motion control algorithm within a quarter-sphere can be derived through mirror projection, greatly simplifying the computational load.

Description

Technical Field

[0001] This invention belongs to the field of motion control for industrial robots, specifically relating to a motion control method for a six-axis robotic arm suitable for hemispherical internal spaces. Background Technology

[0002] During nuclear power plant shutdown and overhaul, maintenance and repair work must be carried out inside containers such as reactor pressure vessels, main pumps, and steam generators. Due to the special nature of nuclear power plant radiation, especially in nuclear island equipment such as the primary side water chamber of the steam generator, which is a hemispherical space (divided into symmetrical quarter-spheres by a partition in the middle), it is a high-dose radiation zone. Coupled with the harsh working conditions of high temperature, the radiation dose and physical exertion of personnel are relatively high during the current manual operation process, and the quality of the work is greatly affected by human factors. The risks of radioactive contamination and human-related risks are also relatively high. Summary of the Invention

[0003] The purpose of this invention is to provide a motion control method for a six-axis robotic arm suitable for hemispherical internal spaces. This method can replace manual labor in performing inspection and maintenance work inside nuclear power plants, addressing the complex working environment of existing nuclear power plant operations. This will completely realize machine-assisted operation in high-risk areas, while improving safety and reliability and reducing human error.

[0004] The technical solution of the present invention is as follows: A motion control method for a six-axis robotic arm suitable for a hemispherical internal space, comprising the following steps:

[0005] Step 1: Implement a point-to-point online motion algorithm;

[0006] Step 2: Robot movement is realized.

[0007] The point-to-point online motion algorithm in step 1 includes the following steps: starting from the beginning of the calculation cycle, the computer updates the motion instructions, uses the forward kinematics matrix transformation module to obtain the current spatial position of the robot, plans the relative spatial motion trajectory, and then uses the inverse kinematics matrix transformation module to plan the motion trajectory of each joint. If the safety criteria are met, the six joints are controlled synchronously. If the safety criteria are violated, the process returns to "planning the relative spatial motion trajectory". The calculation cycle ends when a "stop running" instruction is received; otherwise, it returns to "update motion instructions".

[0008] The robot motion implementation process in step 2 includes: after starting the robot, establishing the robot's forward and inverse kinematics model in the Cartesian coordinate system, then introducing a spherical coordinate system to determine intuitive relative coordinates, receiving motion commands, and obtaining a motion control algorithm within a 1 / 4 sphere; then obtaining another motion control algorithm within a 1 / 4 sphere through mirror projection, and finally obtaining a motion control algorithm within a hemispherical shape.

[0009] Step 2 includes establishing a DH model of the robotic arm and applying forward and inverse kinematics algorithms.

[0010] In the implementation of the algorithm in step 2, multiple solutions are discarded and a single solution is retained, so that the robotic arm can achieve automatic motion control according to a predetermined trajectory within a quarter-sphere; through mirror projection, the robotic arm can be synchronously controlled within another symmetrically distributed quarter-sphere, thereby satisfying the motion control within the hemispherical space.

[0011] The beneficial effects of this invention are as follows: 1) Based on the special RRRPRR configuration of the six-axis robotic arm, a dual-mode spatial system motion mathematical model of Cartesian coordinates and spherical coordinates is constructed, which greatly facilitates the conversion between absolute coordinates and relative coordinates and provides an intuitive method for quantitative analysis of positioning accuracy; 2) The robot forward and inverse solution model is established according to the conventional Cartesian coordinate system motion modeling method, and then the spherical coordinate transformation solution is introduced to unify the model expression under the spherical coordinate system, which serves as the mathematical algorithm basis for real-time operation by the operator; 3) Based on the hemispherical space being divided into two symmetrical quarter-spheres by the middle partition, after obtaining the motion control algorithm in one quarter-sphere, the motion control algorithm in the other quarter-sphere can be derived through mirror projection, which greatly simplifies the amount of computation. Attached Figure Description

[0012] Figure 1 This describes the implementation process of the PTP online motion algorithm.

[0013] Figure 2 A flowchart of a motion control method for a six-axis robotic arm applicable to a hemispherical internal space provided by the present invention;

[0014] Figure 3 This is a schematic diagram of the overall structure of the PPPRPP configuration robot.

[0015] Figure 4 This is a schematic diagram of a hemisphere (divided into two quarter-spheres by a central partition). Detailed Implementation

[0016] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0017] This invention provides a motion control method for a six-axis robotic arm suitable for hemispherical internal spaces, enabling operators to conveniently remotely control the robotic arm to complete inspection and maintenance operations within hemispherical spaces in high-irradiation areas of nuclear power plants. This meets the automation and intelligent maintenance requirements of key equipment in nuclear power plants and further ensures the safe and economical operation of nuclear facilities.

[0018] A motion control method for a six-axis robotic arm suitable for hemispherical internal spaces is proposed to meet the requirement of performing maintenance and repair work in the high-irradiation area of ​​nuclear power plants. Utilizing the special RRRPRR configuration of the six-axis robotic arm, similar to a connecting rod with 3D and 2D ball joints respectively attached to both ends, a dual-mode spatial motion mathematical model in Cartesian and spherical coordinate systems can be constructed. First, the forward and inverse kinematics models of the robot are established according to the conventional Cartesian coordinate system motion modeling method. Then, the spherical coordinate transformation solution is introduced to unify the model expression in the spherical coordinate system, serving as the mathematical algorithm basis for real-time operation by the operator.

[0019] A motion control method for a six-axis robotic arm suitable for hemispherical interior spaces includes the following steps:

[0020] Step 1: Implement the point-to-point online motion algorithm

[0021] like Figure 1 As shown, the implementation steps of the Point-to-Point (PTP) online motion algorithm are as follows: Starting from the beginning of the calculation cycle, the computer updates the motion commands and uses the forward kinematics solution matrix transformation module (i.e., the forward model) to obtain the current robot spatial position, and then plans the relative spatial motion trajectory. Specifically, after obtaining the robot coordinates and target coordinates, the PTP algorithm is used to plan the motion path, enabling the robot to move to the target coordinates. Then, the inverse kinematics solution matrix transformation module (i.e., the spherical coordinate transformation solution) is used to plan the motion trajectory of each joint. If the safety criteria are met (the following are the safety criteria: after a fault alarm, the fault must be eliminated before use; real-time monitoring of parameters such as angle, speed, voltage, and current; cutting off safety output if limits are exceeded; stopping the action if the motion range exceeds limits), then synchronously controlling the movement of the six joints. If the safety criteria are violated, the calculation cycle ends when a "stop running" command is received; otherwise, it returns to "update motion commands".

[0022] Correct solution model:

[0023] Depend on Figure 4 From Table 1, we can see that...

[0024] Table 1 Parameters of the DH Model of the Robotic Arm

[0025] i <![CDATA[a i ]]> <![CDATA[α i ]]> <![CDATA[d i ]]> <![CDATA[θ i ]]> 1 0 -90° <![CDATA[-l1]]> <![CDATA[θ1]]> 2 0 90° <![CDATA[l2]]> <![CDATA[θ2 <!-- 2 -->]]> 3 0 0° 0 <![CDATA[θ3]]> 4 0 90° d 0 5 0 -90° <![CDATA[l3]]> <![CDATA[θ5]]> 6 0 0° <![CDATA[l4]]> <![CDATA[θ6]]>

[0026]

[0027]

[0028]

[0029]

[0030]

[0031]

[0032] Based on equations (1) to (6), the forward kinematics model of the robot gripper end relative to the mounting reference axis can be obtained.

[0033]

[0034] Right now

[0035]

[0036]

[0037]

[0038]

[0039] Where s and c are abbreviations for sin and cos, respectively. and Homogeneous matrices The first to fourth column vectors.

[0040] Therefore, the position of the end load in the global coordinate system is:

[0041]

[0042] Spherical coordinate transformation solution:

[0043] Assuming the robot's mounting reference axis coincides with the spherical coordinate system (in reality, there is a certain translational deviation, but for the sake of simplicity, compensation is not considered here), then we have:

[0044]

[0045] Transformation operations between the spherical coordinate system and the robot's rectangular coordinate system can be performed according to equations (7) and (13).

[0046] Inverse solution model:

[0047] The end load pose is represented as

[0048]

[0049] Then the spatial transformation matrix between coordinate system {0} and coordinate system {6} is:

[0050]

[0051] Then, by using equation (6), we can obtain...

[0052]

[0053] Meanwhile, from equations (6) and (7), we can obtain...

[0054]

[0055] Let l3 = 0, then from equations 12 and 13, we can obtain two sets of solutions.

[0056]

[0057] From equations (6) and (14), we can obtain

[0058]

[0059] Based on the characteristics of this robot's transformation matrix, we have:

[0060]

[0061] Therefore, by solving the system of equations (10) and (18) simultaneously, we can obtain...

[0062]

[0063] or

[0064]

[0065] Or satisfy

[0066] There are infinitely many solutions. Among them

[0067]

[0068] Thus, the analytical solution equations of the robot motion inverse model are obtained.

[0069] Step 2: Realizing Robot Motion

[0070] like Figure 2 As shown, the robot motion implementation process involves starting the robot and establishing forward and inverse models in a Cartesian coordinate system (absolute coordinates), as described in step 1. Then, a spherical coordinate system (relative coordinates) is introduced to determine intuitive relative coordinates. Through real-time operation by the operator, motion commands are received to obtain a motion control algorithm within a 1 / 4 sphere. Then, through mirror projection, another motion control algorithm within a 1 / 4 sphere is obtained, and finally, a motion control algorithm within a hemispherical shape is obtained.

[0071] The six-axis robotic arm is a RRRPRR six-axis serial configuration (e.g.) Figure 3As shown in Table 1, a DH model of the robotic arm is established. The forward and inverse kinematics algorithm is used to avoid spatial interference. To avoid program overflow, multiple solutions can be discarded and a single solution can be retained during algorithm implementation, so that the robotic arm can achieve automatic motion control according to a predetermined trajectory within a quarter sphere. Through mirror projection, the robotic arm can be synchronously controlled within another symmetrically distributed quarter sphere, thereby satisfying the motion control within the hemispherical space.

[0072] The most crucial aspect of robot motion is converting the operator's intuitive commands based on a geodetic coordinate system into synchronized multi-axis movements of the robot. To achieve this, robot motion is implemented using methods such as... Figure 1 The PTP online motion algorithm flow is as follows: In each calculation cycle (or sampling cycle), the human-machine interaction module receives spatial composite commands such as up, down, left, right, forward, backward, and three-axis deflection from the operator, and parses them into commands to move from the current initial point to the next trajectory point. The current spatial pose of the robot is obtained through the forward kinematics of the robot, and the relative spatial displacement trajectory is planned according to the point-to-point relationship. The motion trajectory of each joint is obtained through the inverse kinematics equation of the robot, and the corresponding motion of each joint is implemented synchronously under the condition of meeting the safety criteria.

[0073] like Figure 2 As shown, the algorithm is an embedded online computing process, and the computing cycle, trajectory planning density, and running speed are all adjustable, which enables the robot's movement to exhibit characteristics such as real-time, smoothness, and sensitivity.

[0074] like Figure 3 As shown, based on the PPPPRPP configuration of the robotic arm, a Cartesian coordinate system is established on each joint to obtain the DH model of the robotic arm (DH parameters are shown in Table 1).

[0075] like Figure 4 As shown, based on the hemispherical space being divided into two symmetrical quarter-spheres by a middle partition, after obtaining the motion control algorithm within one quarter-sphere, the motion control algorithm within the other quarter-sphere can be derived through mirror projection. This greatly simplifies the computational load while ensuring motion accessibility.

Claims

1. A motion control method for a six-axis robotic arm suitable for hemispherical internal spaces, characterized in that, Includes the following steps: Step 1: Implement a point-to-point online motion algorithm; The point-to-point online motion algorithm implementation in step 1 includes starting from the beginning of the calculation cycle, updating the motion instructions by the computer, using the forward kinematics solution matrix transformation module to obtain the current spatial position of the robot, and planning the relative spatial motion trajectory. Step 1 uses the inverse kinematics solution matrix transformation module to plan the motion trajectory of each joint. If it meets the safety criteria, the movement of the six joints is controlled synchronously. If it violates the safety criteria, it returns to "planning relative spatial motion trajectory". The calculation cycle ends when a "stop running" command is received. Otherwise, it returns to "update motion command". Step 2: Realizing Robot Motion The robot motion implementation process in step 2 includes establishing a forward and inverse kinematics model of the robot in the Cartesian coordinate system after starting the robot; Step 2 introduces a spherical coordinate system, determines relative coordinates, receives motion commands, and obtains a motion control algorithm within a 1 / 4 sphere; then, through mirror projection, another motion control algorithm within a 1 / 4 sphere is obtained, and finally, a motion control algorithm within a hemispherical shape is obtained. Step 2 includes establishing a DH model of the robotic arm and applying forward and inverse kinematics algorithms; In the implementation of the algorithm in step 2, multiple solutions are discarded and a single solution is retained, so that the robotic arm can achieve automatic motion control according to a predetermined trajectory within a quarter-sphere; through mirror projection, the robotic arm can be synchronously controlled within another symmetrically distributed quarter-sphere, thereby satisfying the motion control within the hemispherical space.

Images