Mechanical arm repetitive motion inverse solution solving method and system considering iterative initial value selection

Abstract

The invention discloses a mechanical arm repetitive motion inverse solution solving method and system considering iteration initial value selection. The problems that when a mechanical arm inverse solution is solved through a numerical iteration method based on a Jacobian matrix, dependence on iteration initial value selection is high, and motion cannot be repeated are solved. Comprising the steps that an accurate reachable working space of the mechanical arm is obtained, a certain number of reference inverse solutions optimized through an operability index are additionally stored in the accurate reachable working space, and an enhanced reachable working space is obtained; according to the expected pose information of the mechanical arm and the enhanced reachable working space, an iteration initial value for solving the inverse solution of the pose is selected and obtained; acquiring a tail end pose error kinematical equation of the mechanical arm at a joint position layer, and deducing to obtain an iterative updating equation of a joint position; and a repeatable motion index function is designed, a joint position iteration updating equation with the repeatable motion function is obtained, and according to the iteration updating equation and the selected iteration initial value, a mechanical arm repeated motion inverse solution solving scheme selected by considering the iteration initial value is obtained.

Description

Technical Field

[0001] This invention relates to the field of inverse kinematics research technology for robots, and in particular to a method and system for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] In the field of robotics, the study of robotic arm kinematics occupies a crucial position, with inverse kinematics solving being a core problem. In recent decades, the problem of inverse kinematics solving has attracted widespread attention from scholars both domestically and internationally. They have conducted in-depth research on how to design efficient and reliable inverse kinematics solving algorithms, proposing many effective solution methods. These methods can be broadly classified into two categories: analytical methods and numerical methods. Analytical methods have significant advantages such as fast solution speed, high accuracy, and the ability to calculate multiple inverse solutions in a single operation. However, they also have the disadvantage of poor versatility due to their dependence on the structural characteristics of the robotic arm. Compared to analytical methods, numerical methods have excellent versatility and can be applied to various robotic arm configurations, thus their application is becoming increasingly widespread. Currently, in engineering practice, numerical iteration methods based on the Jacobian matrix have become a common means of solving the inverse kinematics of robotic arms. However, it is worth noting that such methods have two key problems when solving the inverse kinematics of robotic arms: (1) The efficiency, success rate, and quality of the inverse kinematics solution are highly dependent on the selection of the initial values ​​for iteration. Specifically, if the initial values ​​for iteration are not selected properly, the convergence process of the numerical iteration method will become extremely slow and cannot meet the real-time requirements; it may even fail to converge, directly leading to solution failure. In addition, even if the algorithm eventually converges, the quality of the inverse solution is difficult to guarantee, and there are often problems such as kinematic singularities or joint angles approaching physical limits, which seriously affect the safety and reliability of the robotic arm's movement.

[0004] (2) When using the obtained inverse kinematics solution to control the movement of the robotic arm, there is a non-repetitive motion problem. Specifically, this problem means that when the end effector of the robotic arm performs closed curve motion in Cartesian space, it cannot map the corresponding closed curve in joint space. In other words, although the motion of the end effector is repetitive, the motion in joint space is not. When the task is completed, the joints of the robotic arm may not return to their initial positions, causing the configuration of the robotic arm to shift over time. Especially when performing repetitive tasks, this problem will greatly exacerbate the unpredictability of the robotic arm's motion in configuration space, and will also increase the possibility of collisions with the surrounding environment or operators.

[0005] To address the sensitivity of Jacobi matrix-based numerical iterative methods to initial value selection, existing technologies often employ random methods (such as Monte Carlo methods) to generate random initial values. While this approach can improve the success rate, it suffers from several drawbacks. First, the initial value selection is highly arbitrary and requires iterative calculations to verify its effectiveness. If the initial value is infeasible, it must be reselected, leading to low computational efficiency. Second, the inverse kinematic solution obtained through random initial values ​​is uncertain, potentially resulting in unnatural kinematic arm configurations and severely impacting task reliability. Besides the aforementioned random initial value selection solutions, existing technologies have also attempted to determine initial values ​​using global optimization algorithms such as particle swarm optimization and genetic algorithms. However, these methods generally suffer from high computational complexity and poor real-time performance, making them unsuitable for the real-time control requirements of kinematic arm systems. Furthermore, to address the non-repetitive motion problem in solving the inverse kinematic solution using Jacobi matrix-based methods, various methods for obtaining repeatable motion inverse kinematic solutions have been proposed in existing technologies. However, these methods typically solve non-repetitive motion problems at the joint velocity or acceleration level of the robotic arm. Due to the large number of differentiation and integration operations involved, the algorithms are computationally complex and inefficient. Moreover, in order to obtain the solution for the joint position of the robotic arm, it is necessary to integrate the obtained joint acceleration or joint velocity with respect to time, which inevitably introduces the joint drift problem, resulting in a decrease in the accuracy of the robotic arm's end effector.

[0006] In summary, the widely used Jacobian matrix-based method for solving the inverse kinematics of a robotic arm suffers from sensitivity to the selection of initial iteration values ​​and the problem of non-repeatable motions. Therefore, there is an urgent need to provide a method for obtaining the inverse kinematics of a robotic arm that considers the selection of initial iteration values. Summary of the Invention

[0007] To address the shortcomings of existing technologies, the present invention aims to provide a method and system for obtaining the inverse kinematics of a robotic arm that considers the selection of iterative initial values. When solving the inverse kinematics of the robotic arm, the enhanced reachable workspace enables rapid and reliable selection of iterative initial values, thereby improving the efficiency, success rate, and quality of the solution. By equating the derived first-order differential inverse kinematics equation of the robotic arm to the joint position layer, an iterative update equation for joint position is obtained. Then, a repeatable motion index function is designed to optimize the joint motion in the zero space of the robotic arm. This joint motion is then introduced into the iterative update equation for joint position, constructing an iterative update equation for joint position with repeatable motion functionality. Using this equation avoids the non-repeatable motion problem when obtaining the inverse kinematics of the robotic arm, thus achieving repeatable motion of the robotic arm.

[0008] To achieve the above objectives, the present invention adopts the following technical solution: a method for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values, comprising the following: Establish the workspace accessible to the robotic arm and store relevant information about the workspace accessible to the robotic arm; In the stored information about the reachable workspace of the storage robotic arm, the additional storage setting quantity is enhanced by the reference inverse kinematic solution selected through evaluation of the robotic arm's kinematic performance index, resulting in an enhanced reachable workspace. The first-order differential inverse kinematics equation of the robotic arm is obtained and equivalently applied to the joint position layer; Establish the pose error function of the robotic arm end effector and derive the iterative update equation for the joint position of the robotic arm; Based on the index function of repeatable motion, the gradient projection method is combined with the iterative update equation of the joint position of the robotic arm to construct the iterative update equation of the joint position with repeatable motion function. By combining the enhanced reachable workspace of the robotic arm with the iterative update equations for joint positions that enable repeatable motion, a scheme for obtaining the inverse solution of the robotic arm's repeatable motion, taking into account the selection of initial values ​​for iteration, is obtained.

[0009] The method for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values, as described above, includes the following: establishing the reachable workspace of the robotic arm and storing relevant information about the reachable workspace. The robotic arm's operating space is voxelized, and the resulting voxels are ordered, numbered, and stored in a matrix. middle; An improved Monte Carlo method was used to establish the workspace accessible to the robotic arm and set a precision threshold. , Used to represent The number of reachable pose points that fall into each voxel within the reachable workspace of the robotic arm; In the matrix Found The database is then created by assigning the numbers of all voxels within the reachable workspace of the robotic arm. According to the numbering order, the contents of each voxel mentioned above are... The joint angle position vector information corresponding to each reachable pose point is stored in the database. middle.

[0010] The method for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values, as described above, includes the following aspects in obtaining the enhanced reachable workspace: Obtain the analytical expression for the Jacobian matrix; Combined with the database The information stored in the matrix is ​​arranged according to the information stored in the matrix. The cubes are traversed sequentially by their numbering to calculate the number of cubes within the reachable workspace of the robotic arm, including each voxel. The operability value of each robotic arm configuration is obtained, and the operability index value is obtained; Calculated for each voxel within the reachable workspace of the robotic arm The operability values ​​of each robotic arm configuration are sorted from largest to smallest, and then the top... The manipulator configuration corresponding to each operability value is used as the reference inverse kinematics solution for the manipulator to reach the voxel envelope region, and the generated manipulator joint angle position vector information is stored in the database. middle; Based on the data stored in the database The joint angle position vector information is converted into the pose vector information of the robotic arm end effector. All pose vectors are visualized using discrete point clouds and rotating coordinate systems, and finally, an enhanced reachable workspace for the robotic arm is constructed.

[0011] The method for obtaining the inverse kinematics of a robotic arm considering the selection of iterative initial values, as described above, includes the following steps in obtaining the first-order differential inverse kinematics equations of the robotic arm: When the Jacobian matrix is ​​a full-rank matrix, the inverse kinematic equations of the first-order differential kinematics of the robotic arm are derived based on the Cartesian velocity vector of the end effector and the pseudo-inverse of the Jacobian matrix.

[0012] The above-described method for obtaining the inverse kinematics of a robotic arm considering the selection of iterative initial values ​​involves rewriting the first-order differential inverse kinematics equation of the robotic arm into a form that is differential with respect to time, based on the obtained first-order differential inverse kinematics equation of the robotic arm. At the same time, the differential of time is multiplied on both sides of the equality of the first-order differential inverse kinematics equation of the robotic arm, and finally the inverse kinematics equation of the robotic arm at the joint position layer is obtained.

[0013] The method for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values, as described above, includes the following: Establishing the pose error function of the robotic arm's end effector includes the following: Obtain the position error between the current end position and the desired end position; Obtain the attitude error between the current end-effector attitude and the desired end-effector attitude; A pose error function for the robotic arm end effector is established by combining the position error between the current end position and the desired end position, and the pose error between the current end pose and the desired end pose.

[0014] The method for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values, as described above, includes the following steps for obtaining the iterative update equations for the joint positions of the robotic arm: After establishing the end-effector pose error function of the robotic arm, the kinematic equation of the end-effector pose error is obtained based on the inverse kinematic equation of the robotic arm at the joint position layer. The inverse kinematic equations of the robotic arm are solved using a numerical iterative method. The incremental joint angle position in the kinematic equation of the robot arm's end-effector pose error is calculated using the pseudo-inverse method based on the Jacobian matrix, thus obtaining the iterative update equation of the robot arm's joint position.

[0015] The method for obtaining the inverse kinematics solution of a robotic arm considering the selection of initial values ​​for iteration, as described above, involves constructing an iterative update equation for joint positions with repeatable motion functionality based on an index function for repeatable motion, using the gradient projection method combined with the iterative update equation for the robotic arm joint positions. This includes the following: When the robotic arm is redundant, zero-space optimization technology is used to optimize the zero-space motion of the robotic arm to achieve repeatable motion, and an index function with repeatable motion function is designed. In the formula This is the joint angle position vector corresponding to the initial configuration of the robotic arm. The constant coefficients, For the first The joint angle position vector corresponding to the current configuration of the robotic arm in the next iteration; Using the gradient projection method, the index function with repeatable motion is transformed. The optimized zero-space joint motion is introduced into the joint position iterative update equation of the robotic arm, resulting in a joint position iterative update equation with repeatable motion functionality. In the formula It is the first The joint angle position vector corresponding to the current configuration of the robotic arm in the next iteration. It is the Jacobian matrix of the robotic arm. The pseudo-inverse matrix, It is a Jacobian matrix The projection matrix of the null space, This represents joint motion in null space.

[0016] The method for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values, as described above, combines the enhanced reachable workspace of the robotic arm and the iterative update equations for joint positions with repeatable motion capabilities to obtain the inverse kinematics solution of the robotic arm considering the selection of iterative initial values, and includes the following: Based on the desired position vector of the robotic arm end effector, the voxel mesh is located in the enhanced reachable workspace of the robotic arm to obtain the corresponding voxel number; After obtaining the corresponding voxel number, in the database Find the contents contained in the voxel. Each of the three reference inverse solutions is used to calculate the attitude error between them and the desired pose of the robotic arm. The result The attitude errors are arranged in ascending order, and this order is stored in the database. In the process, the joint angle position vector corresponding to the reference inverse solution with the smallest attitude error is selected as the initial value for iteration when using the numerical iteration method to solve the inverse solution of the current desired pose of the robot arm; When the initial value for iteration is selected, the desired end-effector pose matrix of the redundant robotic arm is given. The joint angle position vector corresponding to the initial configuration and the position error accuracy threshold of the end effector. and attitude error accuracy threshold Obtain the position-level forward kinematic equations to calculate the first In the next iteration, the vector corresponding to the current joint angle position... End pose matrix ; Calculate the position error vector of the robotic arm's end effector. and attitude error vector Determine the stopping condition of the iteration and If both conditions are met, the iteration stops and the current joint position vector is output. This vector is the desired pose matrix of the redundant robotic arm's end effector. and initial joint angle vector If the inverse kinematics solution is not satisfied, the subsequent iteration steps are executed. Based on the joint position iterative update equation with repeatable motion function, the joint angle position vector for the next iteration cycle is calculated. After each update, it is determined whether the pose error of the redundant robotic arm's end effector meets the pose error accuracy threshold. If the condition is not met, repeat the joint position update steps described above until the attitude error accuracy threshold is met. Up to this point, the output current joint position vector is the inverse kinematic solution of the redundant robot arm.

[0017] Secondly, the present invention provides a system for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values, including a computing device configured to: Establish the workspace accessible to the robotic arm and store relevant information about the workspace accessible to the robotic arm; In the stored information about the reachable workspace of the storage robotic arm, the additional storage setting quantity is enhanced by the reference inverse kinematic solution selected through evaluation of the robotic arm's kinematic performance index, resulting in an enhanced reachable workspace. The first-order differential inverse kinematics equation of the robotic arm is obtained and equivalently applied to the joint position layer; Establish the pose error function of the robotic arm end effector and derive the iterative update equation for the joint position of the robotic arm; Based on the index function of repeatable motion, the gradient projection method is combined with the iterative update equation of the joint position of the robotic arm to construct the iterative update equation of the joint position with repeatable motion function. By combining the enhanced reachable workspace of the robotic arm with the iterative update equations for joint positions that enable repeatable motion, a scheme for obtaining the inverse solution of the robotic arm's repeatable motion, taking into account the selection of initial values ​​for iteration, is obtained.

[0018] The beneficial effects of the present invention are as follows: 1) Existing technologies establish a reachable workspace for robotic arms that only characterizes the three-dimensional Cartesian position information reachable by the robotic arm's end effector. Therefore, it cannot be directly used for reliable selection of initial values ​​for numerical iteration in solving the inverse kinematics of the robotic arm. In contrast, the method provided by this invention constructs a six-dimensional reachable workspace that accurately represents the position and attitude information of the robotic arm's end effector. Furthermore, it enhances the workspace by storing an additional number of reference inverse kinematics optimized by the robotic arm's kinematic performance indicators in the discrete data structure of the workspace. This allows for the rapid and reliable selection of a set of reference inverse kinematics as initial values ​​for iteration when using numerical iteration to solve the inverse kinematics of the robotic arm.

[0019] 2) The method for constructing an enhanced reachable workspace for a robotic arm, as provided in this invention, establishes a direct correlation between the initial iteration value and the desired pose of the robotic arm's end effector. This allows for the rapid and reliable selection of a set of reference inverse solutions from the enhanced reachable workspace as initial iteration values ​​when solving the inverse problem of the robotic arm, based on the desired end effector pose information. This solves the problem of blind selection of initial iteration values ​​using random methods in existing technologies, as well as the computational time consumption problem when using global optimization methods, thereby improving the solution efficiency and success rate. Simultaneously, by selecting initial iteration values ​​from the reference inverse solutions optimized by the robotic arm's kinematic performance indicators stored in the enhanced reachable workspace, the quality of the robotic arm inverse solution obtained from these initial values ​​is significantly improved. In summary, the enhanced reachable workspace construction method provided by this invention can effectively improve the computational efficiency, success rate, and quality of the obtained inverse solutions when using the Jacobian matrix-based numerical iteration method to obtain the robotic arm inverse solution.

[0020] 3) The method for obtaining the inverse kinematics solution of the robotic arm in this invention equates the first-order differential inverse kinematics equation of the robotic arm to the joint position layer, obtaining the joint position iterative update equation. Then, by designing a repeatable motion index function, the joint motion in the null space of the robotic arm is optimized. This joint motion is then introduced into the joint position iterative update equation, constructing a joint position iterative update equation with repeatable motion functionality. Using this equation to control the movement of the robotic arm ensures that when the end effector of the robotic arm completes a closed curve task in Cartesian space, each joint angle can accurately return to its initial position, thus achieving repeatable motion. Compared with existing methods that solve the problem of non-repeatable motion at the joint velocity or acceleration layer, the method of this invention directly handles the problem at the joint position level, avoiding the low computational efficiency and joint position drift problems caused by integrating joint velocities or accelerations. Therefore, it can efficiently and accurately obtain the joint position solution, ensuring the efficiency, accuracy, and reliability of obtaining the inverse kinematics solution of the robotic arm. Attached Figure Description

[0021] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0022] Figure 1 A schematic diagram of a seven-degree-of-freedom redundant robotic arm and its forward kinematics model according to one or more embodiments of the present invention; Figure 2 To Figure 1 A schematic diagram of the voxelization process used in the operation space of the robotic arm; Figure 3 To Figure 2 A schematic diagram showing the ordered numbering of voxels obtained after voxelization. Figure 4 for Figure 1 A schematic diagram illustrating the principle of attitude error calculation for the end effector of a robotic arm; Figure 5 A schematic diagram of a robotic arm configuration with low operability value generated by the method based on the pseudo-inverse of the Jacobian matrix; Figure 6 A schematic diagram of a robotic arm configuration with high operability value generated by the method of the present invention; Figure 7 A comparison chart showing the success rates of the method based on the pseudo-inverse of the Jacobian matrix and the method of this invention; Figure 8 This is a comparison chart showing the computational efficiency of the method based on the pseudo-inverse of the Jacobian matrix and the method of this invention; Figure 9 for Figure 1A schematic diagram of the initial configuration of the robotic arm and its closed trajectory in Cartesian space; Figure 10 This is a schematic diagram showing the change of the joint position of the robotic arm over time, obtained by the method based on the pseudo-inverse of the Jacobian matrix. Figure 11 This is a schematic diagram showing the change of the robotic arm joint position over time obtained by the method of the present invention. Detailed Implementation

[0023] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the invention, not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0024] Example 1 Step 1: Establish the forward kinematics model of the robotic arm and derive its position-level forward kinematics equations. The forward kinematics model of the robotic arm should be established using the method described in John J. Craig's "Introduction to Robotics," in which the coordinate system... and coordinate system These represent the base coordinate system and the end effector's coordinate system, respectively. Indicates the first The fixed coordinate system of each joint Let the number of joints be the origin. The derived position-level forward kinematic equations are: (1) in, A vector representing the joint variables of a robotic arm. Indicates the first There are several joint variables, with the superscript indicating the transpose of the vector. Let be the pose matrix representing the end effector coordinate system relative to the base coordinate system. Representing the coordinate system Relative to coordinate system The pose matrix is ​​represented.

[0025] Step 2: Voxelization of the robotic arm's operating space from Step 1, followed by sequential numbering and storage of the resulting voxels. Specifically, Step 21: Use a side length The large cube encloses the operating space of the robotic arm in step one, where... This represents the arm span parameters of the robotic arm. The body center position of the large cube and the robotic arm's base coordinate system are also shown. The origin point of the large cube coincides perfectly with the origin point of the base coordinate system, and the length, width, and height of the cube's sides are respectively aligned with the base coordinate system. of , , The axes remain parallel. The origin coordinates of the robot arm's base coordinate system are represented as follows: Then the coordinates of the eight vertices of the large cube relative to the base coordinate system are as follows: (2) Step 22: Divide the large cube obtained in Step 21 into equal parts along its length, width, and height. The large cube is divided into a total of 1,000 equally spaced parts. There are several cubes (small cubes), each with a side length of 1. This process is called voxelization, and each cube is called a voxel; Steps Two and Three: Starting from the first voxel in the lower left corner of the large cube in Step Two and Two, process the obtained voxels column by column, row by row, and layer by layer. Individual elements are numbered, and a one-dimensional matrix is ​​created. All voxel numbers are stored in this matrix.

[0026] Step 3: Using an improved Monte Carlo method, establish the precise reachable workspace of the robotic arm from Step 1, and create a database to store relevant information about the reachable workspace. Specifically, Step 31: The improved Monte Carlo method should be adopted from the journal article "Xu Zhenbang, Zhao Zhiyuan, He Shuai, et al. Improved Monte Carlo method for solving robot workspace and volume determination [J]. Optics and Precision Engineering, 2018, 26(11):2703-2713." This method is a general numerical method for solving the reachable workspace of a serial robotic arm, which can obtain an accurate reachable workspace. According to the definition of reachable workspace, the improved Monte Carlo method is adopted, and the reachable workspace of the robotic arm is established by combining the position-level positive kinematic equation of the robotic arm obtained in Step 1 and the voxelization information in Step 2. In order to ensure that each region in the established reachable workspace can be accurately described, an accuracy threshold is set. . Used to represent the result obtained in step two. The number of reachable pose points that fall into each voxel within the reachable workspace of the robotic arm; Step 32: Based on the robotic arm's reachable workspace established in Step 31, in the matrix... Find the result obtained in step two. The database is then created by assigning the numbers of all voxels within the reachable workspace of the robotic arm. According to the numbering order, the contents of each voxel mentioned above are... The joint angle position vector information corresponding to each reachable pose point is stored in this database.

[0027] Step 4: In the discrete data structure representing the reachable workspace of the robotic arm, additionally store a certain number of reference inverse kinematic solutions optimized by the kinematic performance indicators of the robotic arm to enhance the workspace, thereby obtaining an enhanced reachable workspace, and store the relevant information of the reachable workspace. Specifically, Step 41: The operability value should be selected as the kinematic performance index for the robotic arm, as it can be used to evaluate the quality of the robotic arm configuration. To calculate the operability value, the Jacobian matrix of the robotic arm is required. Using the vector product method and combining it with the forward kinematic model of the robotic arm from Step 1, the Jacobian matrix is ​​derived. The analytical expression is derived as follows: (3) (4) in, express The List, Representing the coordinate system of Axis in base coordinate system The unit vector represented in the figure. for The zero vector of dimension, Represents the coordinate system of the end effector The origin relative to the coordinate system The position vector is transformed to the base coordinate system. The representation in; Step 42: Obtain the Jacobian matrix After parsing the expression, combine it with the database in step three, two. The information stored in [the system] is stored according to steps two and three. The cubes are numbered sequentially, and calculations are performed to reach the number of voxels contained within each voxel in the workspace. The operability value of each robotic arm configuration is calculated by first substituting the joint angle position vectors of each group into formulas (3) and (4) to obtain the Jacobian matrix, and then using formula (5) as shown below to calculate the corresponding operability index value. : (5) in, Represents the determinant of a matrix; Step 43: Based on the calculation results in Step 42, calculate the values ​​for each voxel within the reachable workspace. The operability values ​​of each robotic arm configuration are sorted from largest to smallest, and then the top... The manipulator configuration corresponding to each operability value is used as the reference inverse kinematics solution for the manipulator to reach the voxel envelope region, and the manipulator joint angle position vector information that generates them is stored in the database created in step 3.2. middle; Step 44: Based on the data stored in the database in Step 32 and Step 43... The joint angle position vector information is converted into the pose vector information of the end effector of the robot arm using the position-level positive kinematic equation in formula (1). Then, all pose vectors are visualized in the form of discrete point cloud and rotating coordinate system, and finally the enhanced reachable workspace of the robot arm is constructed.

[0028] Step 5: Based on the Jacobian matrix, derive the first-order differential inverse kinematics equation of the robotic arm, and apply this equation to the joint position layer. Specifically, Step 51: When the Jacobian matrix obtained in Step 41... When the matrix is ​​full rank, based on the relevant definitions of differential kinematics of robotic arms in John J. Craig's "Introduction to Robotics", the inverse kinematic equations of the first-order differential kinematics of the robotic arm are derived as follows: (6) in, The velocity vector represents the joint variables of the robotic arm. This represents the Cartesian velocity vector of the end effector of the robotic arm. express The pseudo-inverse matrix; Step 52: When calculated using formula (6) Afterwards, to avoid [the spread of the virus] The joint position solution is obtained by integrating relative to time. At this point, the problem of joint angle drift needs to be introduced. This is done by rewriting the first-order differential inverse kinematics equation as a form that is differential with respect to time, and then multiplying both sides of the equation by the differential of time. Finally, the inverse kinematics equation of the robotic arm at the joint position level is obtained as follows: (7) in, and These represent the increments of the joint angle position of the redundant robotic arm in joint space and the corresponding increments of the end effector's pose in Cartesian space, respectively.

[0029] Step 6: Establish the pose error function of the robotic arm's end effector, and then derive the iterative update equation for the joint positions of the robotic arm. Specifically, Step 61: Assume the current joint angle position vector of the robotic arm is... The corresponding end pose matrix is : (8) in, , and These represent the end-effector coordinate systems of the robotic arm's current configuration. of , and Axis in base coordinate system The unit vector is represented in the figure, while This represents the position vector of the origin of the end coordinate system relative to the base coordinate system under this configuration; Next, assume that the desired end-effector pose matrix of the robotic arm is... : (9) in, , and The end-effector coordinate systems represent the desired configuration of the robotic arm. of , and Axis in base coordinate system The unit vector is represented in the figure, while This represents the position vector of the origin of the end coordinate system relative to the base coordinate system under this configuration; The positional error between the current end position and the desired end position can be expressed as: (10) The attitude error between the current end-effector attitude and the desired end-effector attitude can be expressed as: (11) Combining the two types of errors represented in formulas (10) and (11), the pose error function of the robotic arm end effector can be established. for: (12) Step 62: After establishing the end effector pose error function of the robotic arm, based on the inverse kinematic equation of the robotic arm at the joint position layer obtained in Step 52, the kinematic equation of the end effector pose error of the robotic arm can be obtained as follows: (13) Based on formulas (9) and (13), the inverse kinematics equations of the robotic arm can be solved using a numerical iteration method. In this invention, the incremental joint angle position in formula (13) is calculated using a pseudo-inverse method based on the Jacobian matrix, and the iterative update equations for the joint positions of the robotic arm are derived as follows: (14) in, Indicates the first The joint position vector corresponding to the current configuration of the robotic arm in the next iteration.

[0030] Step 7: Using null space optimization techniques, design an index function for repeatable motion. Then, using gradient projection method combined with the iterative update equation of the robotic arm joint positions, construct an iterative update equation for joint positions with repeatable motion functionality. Specifically, Step 71: When the robotic arm is redundant, its zero-space motion can be optimized using zero-space optimization techniques to achieve repeatable motion. To this end, this invention designs an index function with repeatable motion capabilities. Its specific expression is as follows: (15) Among them, coefficient It is a real constant; the larger its value, the faster the optimization speed. This represents the joint position vector corresponding to the initial configuration of the robotic arm when solving the inverse kinematics of the robotic arm.

[0031] Step 72: Using the gradient projection method, transform the index function from Step 71... The optimized zero-space joint motion is introduced into the joint position iterative update equation of the robotic arm in step six-two, resulting in the following joint position iterative update equation with repeatable motion function: (16) in, Representing the Jacobian matrix of the robotic arm The projection matrix of the null space, This represents joint motion in null space.

[0032] Step 8: Combining the enhanced reachable workspace of the robotic arm and the iterative update equations for joint positions with repetitive motion capabilities, design an iterative process to obtain a scheme for solving the inverse kinematics of the robotic arm, taking into account the selection of initial values. Specifically, Step 81: When using the numerical iterative method based on the Jacobian matrix to solve the inverse kinematics of the robotic arm, one can first determine the desired position vector of the robotic arm's end effector. In step four, the corresponding voxel mesh is located in the enhanced reachable workspace. Specifically, the voxel number is calculated using the formula (17) shown below. : (17) (18) (19) (20) in, , and Representing rows, columns, and layers respectively. Represents the absolute value symbol. Indicates the rounding up symbol; Step 82: Obtain the voxel numbering from Step 81. Then, in the database Find the contents contained in the voxel. We obtain several inverse reference solutions, and then use formula (21) to calculate the attitude errors between them and the desired pose of the robotic arm. : (twenty one) in, Indicates the first The rotation matrix at the end of the robotic arm corresponding to each inverse reference solution; Step 83: Based on the calculation results in Step 82, obtain... The attitude errors are arranged in ascending order, and this order is stored in the database. Then, the joint angle position vector corresponding to the reference inverse solution with the smallest attitude error is selected as the initial value for iteration when using the numerical iteration method to solve the inverse solution of the current desired pose of the robot arm; Step 84: Once the initial values ​​for the iteration are obtained, the desired end-effector pose matrix of the robotic arm is given. The joint angle position vector corresponding to the initial configuration and the position accuracy threshold of the end effector. and attitude accuracy threshold Then, firstly, the position-level forward kinematics equations derived in step one are used to calculate the... In the next iteration, the vector corresponding to the current joint angle position... End pose matrix Then, calculate the Jacobian matrix based on the expression derived in step 41. Its projection matrix in null space Next, using formulas (11) and (12) from step six, the position error vector at the end of the robotic arm is calculated respectively. and attitude error vector Secondly, determine the iteration stopping condition. and If both conditions are met, the iteration stops and the current joint angle position vector is output. This vector is the desired pose matrix of the corresponding robotic arm end effector. and initial joint angle vector If the inverse kinematics solution is not satisfied, the subsequent iteration steps are continued; finally, based on the joint position iterative update equation with repeatable motion function obtained in step 72, the joint angle position vector for the next iteration cycle is calculated. After each calculation, it is determined whether the pose error of the robotic arm end effector meets the accuracy threshold. If it does not meet the threshold, the joint position update steps are repeated until the accuracy threshold is met. At this point, the output current joint position vector is the inverse kinematic solution of the robotic arm.

[0033] by Figure 1 Taking the seven-DOF redundant robotic arm shown as an example, the specific implementation steps are as follows: Step 1: Combining Figure 1 The seven-DOF redundant robotic arm shown is modeled using Craig's method, with its forward kinematics model as follows: Figure 1 As shown, in the established model, the coordinate system and coordinate system Let these represent the base coordinate system of the robotic arm and the coordinate system of the end effector, respectively. Indicates the first Based on the above coordinate system setting, the position-level forward kinematics equations of the robotic arm are derived using the fixed coordinate system of each joint:

[0034] in, A vector representing the joint variables of a robotic arm. Indicates the first One joint variable, , Indicates along Shaft from Axis moves to Distance between axes Indicates circling Shaft from The axis rotates to Angle of axis Indicates along Shaft from Axis moves to Distance between axes; Combination Figure 1As shown in Table 1, the DH parameters corresponding to the forward kinematics model of the robotic arm can be calculated by substituting the values ​​of each parameter in the table into the position-level forward kinematics equations. The specific value.

[0035] Table 1. Robotic Arm DH Parameters

[0036] Step Two: Combining Figure 2 As shown, a side length is used. Large cubic envelope Figure 1 The operating space of the robotic arm, the body center of the large cube and Figure 1 Medium robotic arm base coordinate system The origin of the coordinate system coincides with the origin of the coordinate system, and the length, width, and height of the coordinate system are aligned with the direction of the base coordinate system. , , The axes are parallel, and the coordinates of the eight vertices of the large cube relative to the base coordinate system are as follows:

[0037] Next, the large cube is divided equally along its length, width, and height into three sections. The large cube is divided into a total of 1,000 equally spaced parts. A cube, the side length of the cube This process is called voxelization, and the result is as follows: Figure 2 As shown; Combination Figure 2 and Figure 3 As shown, starting from the first voxel in the lower left corner of the large cube, for... Individual elements are numbered sequentially, row by row, and layer by layer. A diagram illustrating the numbering process is shown below. Figure 3 As shown, the first Liede Line number The voxel number corresponding to the layer is Finally, the obtained numbers are stored in the created one-dimensional matrix. middle.

[0038] Step 3: Combining Figure 1 and Figure 3 As shown, based on the definition of reachable workspace, the improved Monte Carlo method proposed in "Xu Zhenbang, Zhao Zhiyuan, He Shuai, et al. Improved Monte Carlo method for solving robot workspace and volume determination [J]. Optics and Precision Engineering, 2018, 26(11):2703-2713" is adopted. Combined with the position-level forward kinematics equations of the seven-DOF redundant robot arm obtained in step one and the voxelization information in step two, the solution is obtained. Figure 1The robotic arm can precisely reach the workspace, based on the accuracy threshold set during the solution process. ; In obtaining Figure 1 After the robotic arm reaches the workspace, it can move within the matrix. Found The database is then created by assigning the voxel numbers to all voxels belonging to the reachable workspace. According to the numbering order, the contents of each voxel mentioned above are... The joint angle position vector information corresponding to each reachable pose point is stored in this database.

[0039] Step 4: Combining Figure 1 The DH parameters in Table 1 are derived using formulas (3) and (4). Figure 1 The robotic arm relative to its base coordinate system Jacobian matrix The parsing expression; Then, combining the workspace accessible by the robotic arm with the database The joint angle position vector information stored in the matrix is ​​arranged according to the one-dimensional matrix. The calculation involves sequentially traversing the voxel numbers to reach the number of voxels contained within the workspace. The operability value of each robotic arm configuration is calculated by first substituting the joint angle position vector of each group into formula (3) to obtain the Jacobian matrix, and then using formula (5) to calculate the corresponding operability index value. Next, the calculated values ​​for each voxel within the reachable workspace are... The operability values ​​of each robotic arm configuration are sorted from largest to smallest. The robotic arm configurations corresponding to the top 100 operability values ​​are then used as the inverse kinematics reference solutions for reaching the voxel envelope region. The joint angle position vector information of the robotic arms that generated these values ​​is stored in a database. middle; Finally, based on the data stored in the database The joint angle position vector information and reference inverse solution information are used. Figure 1 The position-level forward kinematic equations of the robotic arm are converted into pose vector information at the end of the robotic arm. Then, all pose vectors are visualized in the form of discrete point clouds and rotating coordinate systems, and finally, the enhanced reachable workspace of the robotic arm is constructed.

[0040] Step 5: Combining Figure 1 The inverse kinematic equation of the first-order differential of the robotic arm is derived using formula (7) to obtain the inverse kinematic equation of the robotic arm at the joint position level as follows:

[0041] Step Six: Combining Figure 1 The forward kinematics model of the robotic arm and the position-level forward kinematics equations derived in step one are used to establish the pose error function of the robotic arm's end effector using formula (12). Then, based on the inverse kinematic equations of the robotic arm at the joint position layer obtained in step 52, the kinematic equations of the robotic arm end-effector pose error are obtained. Next, the increment of the joint angle position is calculated from these equations using a pseudo-inverse method based on the Jacobian matrix, and the iterative update equations of the robotic arm's joint position are derived as follows:

[0042] Using null-space optimization techniques, a repetitive motion index function is designed. Then, by combining the gradient projection method with the iterative update equation of the robotic arm joint positions, an iterative update equation of the joint positions with repetitive motion functionality is constructed. Step Seven: Combining Figure 1 The robotic arm shown first uses zero-space optimization technology to design an index function with repeatable motion capabilities through formula (15). :

[0043] Then, using the gradient projection method, the result is obtained by applying the index function. Optimized zero-space joint motion introduction Figure 1 The iterative update equation for the joint positions of the robotic arm, which enables repeatable motion, is shown below:

[0044] Step 8: Combining Figures 4-8 As shown, the technical advancement of the present invention is first verified by comparing the results obtained in the inverse solution calculation of the robotic arm by the method based on the pseudo-inverse of the Jacobian matrix and the method provided in this embodiment, from the dimensions of success rate of obtaining the inverse solution, computational efficiency and solution quality.

[0045] To verify the method of this invention from the quality dimension of the obtained inverse solution, a set of pose vectors located within the reachable workspace of the robotic arm was randomly generated as the desired pose. Specifically, the position vector of this pose is... The rotation matrix is Then, the inverse solution corresponding to the desired pose is obtained using a method based on the pseudo-inverse of the Jacobian matrix. The initial values ​​for iteration are obtained randomly during the solution process. The resulting set of inverse solutions is as follows: The corresponding robotic arm configuration is as follows Figure 5 As shown, the operability value of this configuration is 0.068; next, the inverse solution of the desired pose is obtained using the method of the present invention, based on the desired end-effector position vector. The corresponding voxel grid is located in the enhanced reachable workspace of the robotic arm. Specifically, the voxel number is calculated using formula (17). The calculation results are as follows:

[0046]

[0047]

[0048]

[0049] After obtaining the voxel number, in the database Find the contents contained in the voxel. A reference inverse solution, then combined Figure 4 By using formula (11), the attitude error between 100 reference inverse solutions and the desired posture of the robotic arm is calculated respectively. Then, the reference inverse solution with the smallest attitude error is selected as the initial value for iteration when solving the inverse solution of the current desired posture. Finally, a set of inverse solutions is obtained. The corresponding robotic arm configuration is as follows Figure 6 As shown, the operability value of this configuration is 0.364. Comparing the operability values ​​of the inverse solutions obtained by the two methods, the results demonstrate that the method of this invention can significantly improve the quality of the inverse solution of the robotic arm obtained by the numerical iteration method. Combination Figure 7 and Figure 8 As shown, to verify the advancement of the method of this invention from the dimensions of success rate and computational efficiency in obtaining the inverse solution, five sets of experiments were conducted. In each set of experiments, 10,000 sets of random desired poses were selected within the reachable workspace of the robotic arm. When using the method based on the pseudo-inverse of the Jacobian matrix, the initial value of the iteration corresponding to each desired pose is randomly generated; when using the method of this invention, the initial value of the iteration is optimized through the enhanced reachable workspace of the robotic arm. Figure 7 The results show a comparison of the success rates of solving five sets of inverse kinematics experiments using two different methods. Figure 8 The comparison results of the computational efficiency of the two methods for obtaining the inverse solution are shown. The above results fully verify that the method proposed in this invention can effectively solve the problem that the solution performance of numerical iteration method is highly dependent on the initial value of iteration. Next, combined Figures 9-11 As shown, to verify that the method provided by this invention can effectively solve the problem of non-repetitive motion when obtaining the inverse solution based on the Jacobian matrix, the following method is used: Figure 1 Robotic arm tracking shown Figure 9 The closed trajectory in Cartesian space is given, and the initial position vector of the robotic arm joints is set as follows: The positional accuracy threshold of the robotic arm's end effector is Attitude accuracy threshold is And the attitude of the end effector remains unchanged during the tracking process; Obtained by derivation Figure 1 The iterative update equation for the joint position of the robotic arm controls the tracking of the robotic arm. Figure 9 The trajectory in Figure 10 The results show the curves of the joint positions changing over time during the process of the robotic arm repeatedly tracking a pentagonal trajectory 10 times in joint space. It can be seen from the results that the inverse kinematics solution method based on the pseudo-inverse of the Jacobian matrix has a non-repetitive motion problem, that is, after the end effector of the robotic arm completes the tracking of the closed trajectory in Cartesian space, the joints cannot return to their initial positions. Finally, the derivation obtained using this invention... Figure 1 The iterative update equations for joint positions of the robotic arm with repeatable motion functions control the robotic arm tracking. Figure 9 The trajectory in the equation is set as follows: , Figure 11 The figure shows the curves of the joint positions changing over time during the process of the robotic arm repeatedly tracking a pentagonal trajectory 10 times in joint space. As can be seen from the figure, after the end effector of the robotic arm completes the task of tracking the closed curve trajectory in Cartesian space, the joint angles accurately return to their initial positions, thus achieving repeatable motion. The above results prove that the method provided by the present invention can effectively solve the problem of non-repetitive motion when obtaining the inverse solution of the robotic arm based on the Jacobian matrix method.

[0050] Example 2 This embodiment provides a system for obtaining the inverse kinematics solution of a robotic arm considering the selection of iterative initial values, including a computing device, wherein the computing device is selected as a computer, and the computing device is configured as follows: Establish the workspace accessible to the robotic arm and store relevant information about the workspace accessible to the robotic arm; In the stored information about the reachable workspace of the storage robotic arm, the additional storage setting quantity is enhanced by the reference inverse kinematic solution selected through evaluation of the robotic arm's kinematic performance index, resulting in an enhanced reachable workspace. The first-order differential inverse kinematics equation of the robotic arm is obtained and equivalently applied to the joint position layer; Establish the pose error function of the robotic arm end effector and derive the iterative update equation for the joint position of the robotic arm; Based on the index function of repeatable motion, the gradient projection method is combined with the iterative update equation of the joint position of the robotic arm to construct the iterative update equation of the joint position with repeatable motion function. By combining the enhanced reachable workspace of the robotic arm with the iterative update equations for joint positions that enable repeatable motion, a scheme for obtaining the inverse solution of the robotic arm's repeatable motion, taking into account the selection of initial values ​​for iteration, is obtained.

[0051] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for solving inverse kinematics of a robot arm with consideration of selection of initial value of iteration, characterized in that, The method comprises the following steps: establishing a reachable workspace of a robot arm and storing information related to the reachable workspace of the robot arm; in the stored information related to the reachable workspace of the robot arm, a reference inverse kinematics solution evaluated by a set number of kinematics performance indexes of the robot arm is additionally stored to enhance the workspace, and an enhanced reachable workspace is obtained; a first-order differential inverse kinematics equation of the robot arm is obtained and is equivalent to a joint position layer; a pose error function of an end effector of the robot arm is established, and a joint position iterative updating equation of the robot arm is derived; based on an index function of repeatable motion, a gradient projection method is used in combination with the joint position iterative updating equation of the robot arm to construct a joint position iterative updating equation with a repeatable motion function; in combination with the enhanced reachable workspace of the robot arm and the joint position iterative updating equation with the repeatable motion function, a robot arm repeatable motion inverse solution scheme considering an iterative initial value selection is obtained.

2. The method of claim 1, wherein the method further comprises: The method for establishing the reachable workspace of the robot arm and storing the information related to the reachable workspace of the robot arm comprises the following steps: The operation space of the robot arm is voxelized, the voxels obtained by the processing are sequentially numbered and stored in a matrix ; The reachable workspace of the mechanical arm is established by using an improved Monte Carlo method, and a precision threshold is set , for representing the number of reachable pose points falling in each voxel belonging to the reachable workspace of the mechanical arm In the matrix find the number of all voxels belonging to the reachable workspace of the robot arm in the voxel, and then create a database , and store the joint angle position vector information corresponding to each of the reachable pose points contained in each voxel in the database in the order of the number.

3. The method of claim 2, wherein the method further comprises: The enhanced reachable workspace is obtained by the following steps: an analytical expression of a Jacobian matrix is obtained; combining the information stored in the database traversing the reachable workspace of the robotic arm in the order of the cube numbers stored in the matrix obtaining the value of the manipulability index;​ The calculated value of the manipulability of each robot arm configuration in each voxel in the reachable workspace of the robot arm is sorted from large to small, and then the robot arm configuration corresponding to the first The generated joint angle position vector information of the robot arm is stored in the database.​​ According to the joint angle position vector information stored in the database , the pose vector information of the end of the mechanical arm is converted, all the pose vectors are visually displayed in the form of discrete point cloud and rotation coordinate system, and finally the enhanced reachable workspace of the mechanical arm is constructed.

4. The method of claim 1, wherein the method further comprises: The first-order differential inverse kinematics equation of the robot arm is obtained by the following steps: when the Jacobian matrix is a full-rank matrix, an inverse kinematics equation of the first-order differential kinematics of the robot arm is derived according to a Cartesian velocity vector of the end effector of the robot arm and a pseudo-inverse matrix of the Jacobian matrix.

5. The method of claim 1, wherein the method further comprises: According to the obtained first-order differential inverse kinematics equation of the robot arm, the first-order differential inverse kinematics equation is rewritten into a form of differentiation with respect to time, and the differential of time is multiplied on both sides of the first-order differential inverse kinematics equation of the robot arm, and finally an inverse kinematics equation of the robot arm at the joint position layer is obtained.

6. The method of claim 1, wherein the method further comprises: The pose error function of the end effector of the robot arm is established by the following steps: a position error between a current end position and a desired end position is obtained; an attitude error between a current end attitude and a desired end attitude is obtained; the pose error function of the end effector of the robot arm is established in combination with the position error between the current end position and the desired end position and the attitude error between the current end attitude and the desired end attitude.

7. The method of claim 1, wherein the method further comprises: The joint position iterative updating equation of the robot arm is obtained by the following steps: after the end pose error function of the end effector of the robot arm is established, an end pose error kinematics equation of the robot arm is obtained based on the obtained inverse kinematics equation of the robot arm at the joint position layer; a numerical iteration method is used to solve the inverse kinematics equation of the robot arm; a pseudo-inverse method based on the Jacobian matrix is used to calculate an increment of the joint angle position in the end pose error kinematics equation of the robot arm, and a joint position iterative updating equation of the robot arm is obtained.

8. The method of claim 1, wherein the method further comprises: The joint position iterative updating equation with the repeatable motion function is constructed by the following steps: When the robotic arm is redundant, zero-space optimization technology is used to optimize the zero-space motion of the robotic arm to achieve repeatable motion, and an index function with repeatable motion function is designed. In the formula This is the joint angle position vector corresponding to the initial configuration of the robotic arm. The constant coefficients, For the first The joint angle position vector corresponding to the current configuration of the robotic arm in the next iteration; Using the gradient projection method, the index function with repeatable motion is transformed. The optimized zero-space joint motion is introduced into the joint position iterative update equation of the robotic arm, resulting in a joint position iterative update equation with repeatable motion functionality. In the formula It is the first The joint angle position vector corresponding to the current configuration of the robotic arm in the next iteration. It is the Jacobian matrix of the robotic arm. The pseudo-inverse matrix, It is a Jacobian matrix The projection matrix of the null space, This represents joint motion in null space.

9. The method of claim 2, wherein the method further comprises: The joint position iterative update equation with repeatable motion function is constructed by combining the reachable workspace of the enhanced mechanical arm and the gradient projection method based on the index function of repeatable motion and the joint position iterative update equation. According to the desired position vector of the end of the mechanical arm, a voxel grid is positioned in the reachable workspace of the enhanced mechanical arm, and the corresponding voxel number is obtained. After obtaining the number of the corresponding voxel, find the reference inverse solution contained in the voxel in the database , and calculate the pose error between them and the desired pose of the robot arm respectively; ​ The obtained pose errors are arranged in ascending order, and the order is stored in a database The joint angle position vector corresponding to the reference inverse solution with the smallest pose error is selected as the initial value for solving the current desired pose inverse solution of the robot arm by using a numerical iteration method. When the initial value is obtained by selection, the desired end position matrix of the redundant robot arm is given , the initial configuration corresponds to the joint angle position vector , and the position error precision threshold and the attitude error precision threshold of the end effector , the position level forward kinematics equation is obtained to calculate the first iteration, the end position matrix corresponding to the current joint angle position vector ; calculating a position error vector of the end of the robot arm and a pose error vector , determining whether an iteration stop condition and is met, if so, stopping the iteration and outputting a current joint position vector , which is the inverse kinematics solution of a desired pose matrix of the end of the redundant robot arm and an initial joint angle vector , if not, continuing with a subsequent iteration step; According to the joint position iterative update equation with the repeatable motion function, the joint angle position vector of the next iteration period is calculated After each update is completed, it is determined whether the redundant manipulator end position and posture error satisfies the posture error precision threshold If not, the joint position update step is repeated until the posture error precision threshold is satisfied At this time, the output current joint position vector is the kinematics inverse solution of the redundant manipulator 10. A system for solving inverse kinematics of a robot arm with consideration of selection of initial values of iterations, characterized by, The computing device is configured to: establish the reachable workspace of the mechanical arm and store the information related to the reachable workspace of the mechanical arm; In the stored information related to the reachable workspace of the mechanical arm, a certain number of reference inverse kinematics solutions evaluated by the kinematic performance index of the mechanical arm are additionally stored to enhance the workspace, and the enhanced reachable workspace is obtained; obtain the first-order differential inverse kinematics equation of the mechanical arm and equivalent it to the joint position layer; establish the pose error function of the end effector of the mechanical arm, and derive the joint position iterative update equation of the mechanical arm; Based on the index function of repeatable motion, the joint position iterative update equation with repeatable motion function is constructed by combining the gradient projection method and the joint position iterative update equation; The joint position iterative update equation with repeatable motion function is constructed by combining the reachable workspace of the enhanced mechanical arm and the gradient projection method based on the index function of repeatable motion and the joint position iterative update equation.

Images