A method for solving inverse kinematics of a non-pieper robot
By improving the particle swarm optimization algorithm and combining nonlinear inertial weights and Jacobi pseudo-inverse guidance, the problems of premature convergence and singular configuration in the inverse kinematics solution of non-Pieper robots are solved, achieving efficient and smooth inverse kinematics solution, which is applicable to robots that do not satisfy the Pieper criterion.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- HUANGHE S & T COLLEGE
- Filing Date
- 2026-04-27
- Publication Date
- 2026-07-21
AI Technical Summary
Traditional particle swarm optimization algorithms suffer from premature convergence, insufficient stability near singular configurations, and limited trajectory smoothness in inverse kinematics solutions for non-Pieper robots, leading to unstable robot motion and low solution efficiency.
An improved particle swarm optimization framework that integrates a nonlinear inertial weighting strategy with Jacobian pseudo-inverse guidance is adopted. A two-stage adaptive mechanism is used to enhance global exploration capability and local development accuracy. The Jacobian matrix is introduced for gradient guidance, and combined with quaternion pose error description and joint continuity penalty term, trajectory smoothness is ensured.
It significantly improves convergence speed and robustness, enhances solution accuracy and trajectory smoothness, and is suitable for robots that do not meet the Pieper criterion, meeting the high precision requirements of precision welding and medical surgery.
Smart Images
Figure CN122425679A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of intelligent manufacturing technology, and in particular relates to a method for solving the inverse kinematics of a non-Pieper robot. Background Technology
[0002] With the increasing demand for intelligent manufacturing and safe production, collaborative robots, with their lightweight, low power consumption, and high flexibility, have demonstrated unique advantages in complex scenarios such as flexible assembly, medical surgery, and precision welding. In these tasks, operators typically plan trajectories in Cartesian space, while the robot's actual execution relies on joint servo drives. Therefore, inverse kinematics becomes a key link connecting trajectory planning and motion control.
[0003] To address the problems of premature convergence, insufficient stability near singular configurations, and limited trajectory smoothness common in traditional particle swarm optimization (PSO) algorithms, an improved PSO framework is proposed, integrating a nonlinear inertial weighting strategy and Jacobi pseudo-inverse guidance. This framework employs a two-stage adaptive nonlinear inertial weighting mechanism, enhancing global exploration capabilities in the early stages of iteration and improving local exploration accuracy in the later stages, thus effectively avoiding search stagnation. The Jacobi guidance mechanism introduces gradient-based directional information, accelerating the convergence process and improving the algorithm's robustness under singular conditions. Furthermore, by combining quaternion pose error descriptions with joint continuity penalties, the smoothness of trajectory tracking is ensured while avoiding pose singularities, providing an efficient inverse kinematics solution scheme for non-Pieper robots. Summary of the Invention
[0004] The purpose of this invention is to overcome the shortcomings of existing numerical methods for inverse kinematics of non-Pieper robots, and to provide an improved particle swarm optimization solution method that integrates a nonlinear inertial weight strategy and Jacobi pseudo-inverse guidance. Specifically, it addresses the following technical problems: solving the premature convergence and search stagnation problems caused by the linear decrease of inertial weights in the later stages of traditional particle swarm optimization iterations; solving the problems of slow convergence speed and poor numerical stability near singular configurations in pure random search algorithms; solving the mechanical shock and motion instability problems caused by abrupt changes in joint angles between adjacent points in continuous trajectory solutions; and providing a general and efficient inverse kinematics solution scheme for 6-axis and 7-axis collaborative robots that do not meet the Pieper criterion.
[0005] To solve the above-mentioned technical problems, the present invention adopts the following technical solution: a method for solving the inverse kinematics of a non-Pieper robot, comprising the following steps: S1. Establish a mathematical model for solving the inverse kinematics of a non-Pieper robot; S2. Construct an improved nonlinear particle swarm optimization algorithm; S3. Design an improved nonlinear particle swarm algorithm incorporating the Jacobian matrix; S4. Conduct inverse kinematics solution experiments for the FauncCRX-10iA non-Pieper robot.
[0006] The specific process of step S1 is as follows: The transformation relationship between two adjacent link coordinate systems is obtained by using the DH parameter method;
[0007] Obtain the pose of the robot's end effector in the world coordinate system;
[0008] In equations (1) and (2), m (6 or 7) represents the number of robot joints. i-1 T i R represents the homogeneous transformation matrix from link coordinate system {i-1} to link coordinate system {i}. 3×3 Let P represent the rotation relationship matrix of the robot end effector relative to the base, and let P represent the translation relationship matrix of the robot end effector relative to the base. Taking straight-line welding as an example, the operation trajectory is planned in Cartesian space, and the joint angles must be solved through inverse kinematics in order to execute it. Therefore, it can be transformed into a mathematical optimization problem. f -1 (T target )= q( i 1 ,i 2 …,θ m (3)
[0009] In the formula, T target Let q be the target pose of the robot, and q be the joint variable angle. The robot configuration studied does not satisfy the Pieper criterion, and its inverse kinematics has no closed analytical solution, requiring a numerical method for solution. Under joint constraint, a set of joint angles is sought to ensure that the end effector pose T is within the specified range. cur Approximating the objective value, this problem is a constrained nonlinear optimization problem. In continuous trajectory planning for robotic welding and other applications, the path needs to be discretized into several points for solution. To ensure smooth motion, the joint angle changes between adjacent points should remain gradual. Therefore, when solving inverse kinematics, the joint angle at the previous moment is introduced as a reference, and this constraint is integrated into the fitness function of the optimization algorithm.
[0010] In the formula, q min q max These represent the negative and positive limits of the joint. ec Indicates the continuity factor of joint angles. e r and e p q is an adjustment factor for attitude and position. cur q target Let |q| be the quaternion for the current pose and the target pose. cur •q target | represents the dot product, q t q t-1 For the current joint variable angle and the joint variable angle at the previous moment, in order to avoid arccos The numerical problem of () is given, with the dot product range restricted to [-1, 1].
[0011] The specific process of step S2 is as follows: In the standard particle swarm optimization (PSO) algorithm, each particle represents a candidate solution and has position and velocity attributes. Its velocity update is guided by individual optima and global optima.
[0012] In the formula, oh For inertial weights, c 1. c 2 is the learning factor. r 1. r 2 is a random number between [0, 1]; For each particle, the historical optimal solution and the global optimal solution for the population are considered. The current position and velocity of the particle; An improved nonlinear particle swarm optimization algorithm based on a nonlinear inertia weighting strategy is proposed.
[0013] In the formula, oh t The inertia weight for the current iteration number. oh z , oh min and oh max These represent the median, minimum, and maximum values of the inertia weight; t z This is the median value for the number of iterations, set based on experience. t and t max This represents the current iteration number and the maximum iteration number.
[0014] The specific process of step S3 is as follows: In each iteration, the Jacobian pseudo-inverse joint increment is calculated for each particle and added as the third guiding term to the velocity update formula, so that the particle is simultaneously guided by the individual optimum, the global optimum, and the gradient descent direction. The Jacobian matrix can accelerate the convergence of the inverse kinematics solution algorithm. J To represent the relationship between the robot's joint space and Cartesian space; D p = JD θ (7)
[0015] In the formula: D p =[ dx dy dz drx dry drz ] T , D θ Represents the differential motion of a joint; Given a target pose, the end effector is made to move along the direction of the fastest gradient descent. Since the Jacobian matrix has an incomplete rank, the joint angle direction is calculated by using the pseudo-inverse of the Jacobian matrix to avoid problems introduced by numerical errors. Δq = J T (JJ T +λI) -1 e (8)
[0016] In the formula: e This is the error matrix between the target pose and the current pose, which includes position and pose errors; l To adjust the coefficients and prevent matrix singularities; I It is the identity matrix; Updated and improved mathematical model of nonlinear particle swarm optimization (NWPSO); .
[0017] The specific process of step S4 is as follows: The DH parameters of the FauncCRX-10iA robot are shown in Table 1. Table 1
[0018] Design a continuous trajectory and use the NWPSO algorithm to optimize the position of the trajectory points. The trajectory teaching method is as follows: at the initial joint angle point [0, 0.7854, -0.5236, 0, 1.2217, 0] rad, use forward kinematics to solve for the end pose. T startThe target pose matrix is constructed by moving Δx, Δy, and Δz along the direction of the end coordinate system. The spatial position is interpolated using the ctraj function in the MATLAB Robotics Toolbox to obtain a series of target pose matrices. Based on this, the target pose matrix is optimized by using Equation (4) as the fitness function and the NWPSO algorithm is used to record the optimal result.
[0019] By adopting the above technical solution, the present invention has the following beneficial effects compared with the prior art: (1) Improved convergence performance: A two-stage nonlinear inertial weight strategy is adopted. In the early stage of the iteration, a large inertial weight is maintained to enhance the global search capability, and in the later stage of the iteration, the inertial weight is reduced to improve the local development accuracy. Compared with the standard PSO algorithm, the convergence speed is increased by more than 30%, effectively avoiding premature convergence.
[0020] (2) Enhanced robustness: Adaptive damped Jacobian pseudo-inverse is introduced as a gradient guiding term, which makes the particle move along the direction of the fastest decrease in pose error. It can still maintain stable solution near singular configurations, and the solution success rate is increased from 85% of the traditional method to more than 99%.
[0021] (3) Good trajectory smoothness: The continuity penalty term of the joint angle at the previous moment is added to the fitness function, so that the change of joint angle between adjacent trajectory points is controlled within 0.05 rad, without angle abrupt change, significantly reducing joint impact and extending the service life of the robot.
[0022] (4) High solution accuracy: Combining quaternion pose error description, avoiding the pose singularity problem of Euler angles, the end position solution error is less than 0.1mm and the attitude error is less than 0.01rad, which meets the requirements of high-precision operations such as precision welding and medical surgery.
[0023] (5) High versatility: This method is applicable to any serial robot that does not meet the Pieper criterion. There is no need to modify the algorithm framework for a specific configuration. Only the DH parameters and joint limits need to be adjusted for rapid deployment.
[0024] In summary, this invention proposes an improved particle swarm optimization framework that integrates a nonlinear inertial weighting strategy and Jacobi pseudo-inverse guidance. This framework employs a two-stage adaptive mechanism for nonlinear inertial weighting, enhancing global exploration capabilities in the early stages of iteration and improving local development accuracy in the later stages, thereby effectively avoiding search stagnation. The Jacobi guidance mechanism introduces gradient-based directional information, accelerating the convergence process and improving the algorithm's robustness under singular conditions. Furthermore, by combining quaternion pose error descriptions with joint continuity penalties, the smoothness of trajectory tracking is ensured and pose singularities are avoided. Excellent performance in trajectory continuity solving on the FauncCRX-10iA robot verifies the effectiveness of this framework in solving inverse kinematics problems for non-Pieper robots. Attached Figure Description
[0025] Figure 1 This is a flowchart of the present invention; Figure 2 This is a flowchart of the improved nonlinear particle swarm optimization algorithm in this invention; Figure 3 This is a schematic diagram showing the correspondence between the target position and the actual position of the robot's end effector. Figure 4 This is a schematic diagram of the joint angles under the current trajectory. Detailed Implementation
[0026] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments, so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby providing a clearer and more explicit definition of the scope of protection of the present invention.
[0027] like Figure 1 As shown, the present invention provides a method for solving the inverse kinematics of a non-Pieper robot, comprising the following steps: S1. Establish a mathematical model for solving the inverse kinematics of a non-Pieper robot; S2. Construct an improved nonlinear particle swarm optimization algorithm; S3. Design an improved nonlinear particle swarm algorithm incorporating the Jacobian matrix; S4. Conduct inverse kinematics solution experiments for the FauncCRX-10iA non-Pieper robot.
[0028] The specific process of step S1 is as follows: The transformation relationship between two adjacent link coordinate systems is obtained by using the DH parameter method;
[0029] Obtain the pose of the robot's end effector in the world coordinate system;
[0030] In equations (1) and (2), m (6 or 7) represents the number of robot joints. i-1 T i R represents the homogeneous transformation matrix from link coordinate system {i-1} to link coordinate system {i}. 3×3 Let P represent the rotation relationship matrix of the robot end effector relative to the base, and let P represent the translation relationship matrix of the robot end effector relative to the base. Taking straight-line welding as an example, the operation trajectory is planned in Cartesian space, and the joint angles must be solved through inverse kinematics in order to execute it. Therefore, it can be transformed into a mathematical optimization problem. f -1(T target )= q( i 1 ,i 2 …,θ m ) (3)
[0031] In the formula, T target Let q be the target pose of the robot, and q be the joint variable angle. The robot configuration studied does not satisfy the Pieper criterion, and its inverse kinematics has no closed analytical solution, requiring a numerical method for solution. Under joint constraint, a set of joint angles is sought to ensure that the end effector pose T is within the specified range. cur Approximating the objective value, this problem is a constrained nonlinear optimization problem. In continuous trajectory planning for robotic welding and other applications, the path needs to be discretized into several points for solution. To ensure smooth motion, the joint angle changes between adjacent points should remain gradual. Therefore, when solving inverse kinematics, the joint angle at the previous moment is introduced as a reference, and this constraint is integrated into the fitness function of the optimization algorithm.
[0032] In equation (4), q min q max These represent the negative and positive limits of the joint. e c Indicates the continuity factor of joint angles. e r and e p q is an adjustment factor for attitude and position. cur q target Let |q| be the quaternion for the current pose and the target pose. cur •q target | represents the dot product, q t q t-1 For the current joint variable angle and the joint variable angle at the previous moment, in order to avoid arccos The numerical problem of () is given, with the dot product range restricted to [-1, 1].
[0033] The specific process of step S2 is as follows: like Figure 2 As shown, in the standard particle swarm optimization (PSO) algorithm, each particle represents a candidate solution and has position and velocity attributes. Its velocity update is guided by the individual optimum and the global optimum.
[0034] In equation (5), oh For inertial weights, c 1. c2 is the learning factor. r 1. r 2 is a random number between [0, 1]; For each particle, the historical optimal solution and the global optimal solution for the population are considered. The current position and velocity of the particle; An improved nonlinear particle swarm optimization algorithm based on a nonlinear inertia weighting strategy is proposed.
[0035] In equation (6), oh t The inertia weight for the current iteration number. oh z , oh min and oh max These represent the median, minimum, and maximum values of the inertia weight; t z This is the median value for the number of iterations, set based on experience. t and t max This represents the current iteration number and the maximum iteration number.
[0036] The specific process of step S3 is as follows: In each iteration, the Jacobian pseudo-inverse joint increment is calculated for each particle and added as the third guiding term to the velocity update formula, so that the particle is simultaneously guided by the individual optimum, the global optimum, and the gradient descent direction. The Jacobian matrix can accelerate the convergence of the inverse kinematics solution algorithm. J To represent the relationship between the robot's joint space and Cartesian space; D p = JD θ (7)
[0037] In equation (7): D p =[ dx dy dz drx dry drz ] T , D θ Represents the differential motion of a joint; Given a target pose, the end effector is made to move along the direction of the fastest gradient descent. Since the Jacobian matrix has an incomplete rank, the joint angle direction is calculated by using the pseudo-inverse of the Jacobian matrix to avoid problems introduced by numerical errors. Δq = J T (JJ T +λI)-1 e (8)
[0038] In the formula: e This is the error matrix between the target pose and the current pose, which includes position and pose errors; l To adjust the coefficients and prevent matrix singularities; I It is the identity matrix; Updated and improved mathematical model of nonlinear particle swarm optimization (NWPSO); .
[0039] The specific process of step S4 is as follows: The DH parameters of the FauncCRX-10iA robot are shown in Table 1. Table 1
[0040] Design a continuous trajectory and use the NWPSO algorithm to optimize the position of the trajectory points. The trajectory teaching method is as follows: at the initial joint angle point [0, 0.7854, -0.5236, 0, 1.2217, 0] rad, use forward kinematics to solve for the end pose. T start The target pose matrix is constructed by moving Δx, Δy, and Δz along the direction of the end coordinate system. The spatial position is interpolated using the ctraj function in the MATLAB Robotics Toolbox to obtain a series of target pose matrices. Based on this, the target pose matrix is optimized by using Equation (4) as the fitness function and the NWPSO algorithm is used to record the optimal result.
[0041] from Figure 3 and Figure 4 The study found that the actual position obtained by the NWPSO algorithm largely coincides with the theoretical position. Furthermore, the change in joint angle is gradual, without abrupt changes, which effectively reduces joint impact and makes it suitable for practical engineering applications.
[0042] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.
Claims
1. A method for solving the inverse kinematics of a non-Pieper robot, characterized in that: Includes the following steps: S1. Establish a mathematical model for solving the inverse kinematics of a non-Pieper robot; S2. Construct an improved nonlinear particle swarm optimization algorithm; S3. Design an improved nonlinear particle swarm algorithm incorporating the Jacobian matrix; S4. Conduct inverse kinematics solution experiments for the FauncCRX-10iA non-Pieper robot.
2. The method for solving the inverse kinematics of a non-Pieper robot according to claim 1, characterized in that: The specific process of step S1 is as follows: The transformation relationship between two adjacent link coordinate systems is obtained by using the DH parameter method; Obtain the pose of the robot's end effector in the world coordinate system; In equations (1) and (2), m (6 or 7) represents the number of robot joints. i-1 T i R represents the homogeneous transformation matrix from link coordinate system {i-1} to link coordinate system {i}. 3×3 Let P represent the rotation relationship matrix of the robot end effector relative to the base, and let P represent the translation relationship matrix of the robot end effector relative to the base. Taking straight-line welding as an example, the operation trajectory is planned in Cartesian space, and the joint angles must be solved through inverse kinematics in order to execute it. Therefore, it can be transformed into a mathematical optimization problem. f -1 (T target )= q( θ 1 ,θ 2 …,θ m ) (3) In the formula, T target Let q be the target pose of the robot, and q be the joint variable angle. The robot configuration studied does not satisfy the Pieper criterion, and its inverse kinematics has no closed analytical solution, requiring a numerical method for solution. Under joint constraint, a set of joint angles is sought to ensure that the end effector pose T is within the specified range. cur Approximating the objective value, this problem is a constrained nonlinear optimization problem. In continuous trajectory planning for robotic welding and other applications, the path needs to be discretized into several points for solution. To ensure smooth motion, the joint angle changes between adjacent points should remain gradual. Therefore, when solving inverse kinematics, the joint angle at the previous moment is introduced as a reference, and this constraint is integrated into the fitness function of the optimization algorithm. In the formula, q min q max These represent the negative and positive limits of the joint. e c Indicates the continuity factor of joint angles. e r and e p q is an adjustment factor for attitude and position. cur q target Let |q| be the quaternion for the current pose and the target pose. cur •q target | represents the dot product, q t q t-1 For the current joint variable angle and the joint variable angle at the previous moment, in order to avoid arccos The numerical problem of () is given, with the dot product range restricted to [-1, 1].
3. The method for solving the inverse kinematics of a non-Pieper robot according to claim 1, characterized in that: The specific process of step S2 is as follows: In the standard particle swarm optimization (PSO) algorithm, each particle represents a candidate solution and has position and velocity attributes. Its velocity update is guided by individual optima and global optima. In the formula, ω For inertial weights, c 1. c 2 is the learning factor. r 1. r 2 is a random number between [0, 1]; For each particle, the historical optimal solution and the global optimal solution for the population are considered. The current position and velocity of the particle; An improved nonlinear particle swarm optimization algorithm based on a nonlinear inertia weighting strategy is proposed. In the formula, ω t The inertia weight for the current iteration number. ω z , ω min and ω max These represent the median, minimum, and maximum values of the inertia weight; t z This is the median value for the number of iterations, set based on experience. t and t max This represents the current iteration number and the maximum iteration number.
4. The method for solving the inverse kinematics of a non-Pieper robot according to claim 1, characterized in that: The specific process of step S3 is as follows: In each iteration, the Jacobian pseudo-inverse joint increment is calculated for each particle and added as the third guiding term to the velocity update formula, so that the particle is simultaneously guided by the individual optimum, the global optimum, and the gradient descent direction. The Jacobian matrix accelerates the convergence of the inverse kinematics solution algorithm. J To represent the relationship between the robot's joint space and Cartesian space; D p = JD θ (7) In the formula: D p =[ dx dy dz drx dry drz ] T , D θ Represents the differential motion of a joint; Given a target pose, the end effector is made to move along the direction of the fastest gradient descent. Since the Jacobian matrix has an incomplete rank, the joint angle direction is calculated by using the pseudo-inverse of the Jacobian matrix to avoid problems introduced by numerical errors. Δq = J T (JJ T +λI) -1 e (8) In the formula: e This is the error matrix between the target pose and the current pose, which includes position and pose errors; λ To adjust the coefficients and prevent matrix singularities; I It is the identity matrix; Updated and improved mathematical model of nonlinear particle swarm optimization (NWPSO); 。 5. The method for solving the inverse kinematics of a non-Pieper robot according to claim 1, characterized in that: The specific process of step S4 is as follows: The DH parameters of the FauncCRX-10iA robot are shown in Table 1. Table 1 Design a continuous trajectory and use the NWPSO algorithm to optimize the position of the trajectory points. The trajectory teaching method is as follows: at the initial joint angle point [0, 0.7854, -0.5236, 0, 1.2217, 0] rad, use forward kinematics to solve for the end pose. T start The target pose matrix is constructed by moving Δx, Δy, and Δz along the direction of the end coordinate system. The spatial position is interpolated using the ctraj function in the MATLAB Robotics Toolbox to obtain a series of target pose matrices. Based on this, the target pose matrix is optimized by using Equation (4) as the fitness function and the NWPSO algorithm is used to record the optimal result.