Redundant mechanical arm remote operation control method based on inverse kinematics

Abstract

The invention discloses a redundant mechanical arm remote operation control method based on inverse kinematics, and aims at solving the problems that a traditional method is slow in convergence, sensitive to an initial value, insufficient in singular point processing and the like. The method comprises the following steps: firstly, establishing a redundant robot kinematics model, and determining a target pose and a joint configuration vector of an execution end; constructing an auxiliary continuation function consisting of a target function, an initial pose error function and an auxiliary function; constructing a fusion function by adopting nonlinear homotopy parameters based on a continuation method to realize smooth transition; an inverse kinematics problem is converted into a quadratic programming problem with joint velocity constraints, and an objective function and constraint conditions are defined; and iteratively updating the joint configuration through a quadratic programming solver until a preset precision convergence condition is met. According to the method, the advantages of the continuation method and quadratic programming are fused, the calculation speed is increased by more than one time compared with the Newton-Raphson method, multiple constraints can be processed, and the method is suitable for real-time motion control scenes of robots such as redundant mechanical arms.

Description

Technical Field

[0001] This invention relates to the technical field of robot motion control, and specifically to a redundant robotic arm remote control method based on inverse kinematics. Background Technology

[0002] Inverse kinematics (IK) is a fundamental problem in the field of robot motion control. Due to the geometry of robotic arms and the complexity of the nonlinear trigonometric equations from Cartesian space to joint space, solving this problem typically requires sophisticated methods. This problem becomes even more complex when the robot has redundancy—that is, more than the six coordinates required to reach any point in the three-dimensional workspace. Redundant robot joints, due to their multi-degree-of-freedom nature, can provide multiple solutions for completing the same task, thus offering greater flexibility in task execution. Accurate and optimal inverse kinematics solutions play a crucial role in human-robot collaboration, precision assembly, and complex and hazardous tasks.

[0003] Currently, the classic numerical algorithms for solving inverse kinematics problems are the Newton-Raphson (NR) method and the damped least squares (DLS) method, namely the Levenberg–Marquardt (LM) algorithm, etc.

[0004] However, existing inverse kinematics solutions, when faced with complex constraints such as joint velocities, rely heavily on the selection of initial estimates for efficiency and stability. This is especially true when the initial and target poses differ significantly, leading to slow or non-convergent convergence, which fails to meet the demands of real-time control. Furthermore, existing algorithms may not maintain stable efficiency or accuracy under different robot structures and task scenarios, exhibiting severely insufficient robustness. Summary of the Invention

[0005] To overcome the shortcomings of existing robot motion control technologies, this invention proposes an efficient quadratic programming method for kinematic control, which achieves high solution efficiency and accuracy, is independent of initial values, has strong generalization ability, and is highly robust. This is a redundant remote control method for robotic arms based on inverse kinematics.

[0006] The specific technical solution is as follows:

[0007] A redundant robotic arm teleoperation control method based on inverse kinematics includes the following steps:

[0008] S1. Establish the kinematic model of the redundant robotic arm and determine the target pose of the end effector in the m-dimensional task space. And the joint configuration vector q of the n-degree-of-freedom serial robotic arm, where m≤n, and the positive kinematic function of the robotic arm is Φ(q), satisfying ) , Let t be the pose vector in Cartesian space, t be the motion time, and Φ be the positive kinematics function of the serial robotic arm.

[0009] S2. Construct auxiliary extension functions, including the objective function. Initial pose error function and auxiliary functions ,in To analyze the Jacobian matrix, For joint configuration velocity, For the end effector speed error, Initial estimates for the joints;

[0010] S3. Constructing a fusion function based on the extension method Hotopism parameter , ;

[0011] S4. Transform the inverse kinematics problem into a quadratic programming problem with joint velocity constraints, with the objective function being: The constraints are ,in , This is the lower limit vector of joint velocity. This is the upper limit vector of joint velocity;

[0012] S5. Solve the above quadratic programming problem using a quadratic programming solver, iteratively updating the joint configuration vector q until the convergence condition is met. To preset the precision, output the final joint configuration vector.

[0013] Furthermore, the lower limit of joint velocity in S4 and The upper limits satisfy the following: , ;

[0014] in and These are the lower and upper limits of the range of motion of the i-th joint, respectively. and Let be the lower and upper limits of the angular velocity constraints for the i-th joint, respectively. For time intervals.

[0015] Furthermore, the inequality constraint matrix of the quadratic programming problem in S5 satisfies: , ,in It is an n-order identity matrix.

[0016] Furthermore, the preset precision in the convergence conditions The initial value is set to It can be adjusted to a higher precision level as needed.

[0017] Furthermore, during the iteration process, the actual pose of the end effector is updated in real time through the positive kinematics function, and the error between the actual pose and the target pose is calculated until the error meets the convergence condition or reaches the preset maximum number of iterations.

[0018] Furthermore, the quadratic programming solver uses the ProxQP solver, the Jacobian matrix is ​​obtained through the PinocchioPython interface, and all calculations are performed using double-precision arithmetic.

[0019] Furthermore, it also includes introducing a damping effect into the Jacobian matrix, and correcting the Jacobian matrix based on the Leweenberg-Marquardt method. , where k is the damping factor and I is the identity matrix.

[0020] Furthermore, when iteratively updating the joint configuration vector, the following conditions are met: ,in , Unit of time.

[0021] Furthermore, in the kinematic model of the redundant robotic arm, the pose of the actuator is described by a homogeneous transformation matrix, which consists of a rotation matrix and a translation vector.

[0022] Furthermore, the method is applied to a teleoperated robot system, wherein the communication frequency between the robot system and the external control device is not less than 500Hz, thereby achieving real-time motion control.

[0023] The beneficial effects of this invention are as follows:

[0024] (1) Improve computing power: The improved technology needs to maintain high convergence efficiency even when the initial posture and the target posture are significantly different, so that it can run efficiently in real-time control scenarios and maintain high-precision solution results.

[0025] (2) Eliminate dependence on initial values: As long as it is within the range of each joint, no matter how the initial value is selected, the improved technology can solve the angle value of each joint within the joint limit.

[0026] (3) Improve robustness: The improved algorithm needs to maintain stable and reliable performance under different environmental conditions, including but not limited to situations such as excessively small joint limits and excessive redundancy.

[0027] (4) Improve generalization ability: By optimizing the algorithm structure and parameter settings, the improved algorithm should have better generalization ability and be able to adapt to different robot structures and task requirements, so as to maintain excellent performance in a wide range of application scenarios. Attached Figure Description

[0028] Figure 1 This is a chart comparing the experimental results of this invention with other inverse kinematics (IK) methods. Detailed Implementation

[0029] The preferred embodiments of the present invention will now be described in detail 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.

[0030] Example:

[0031] A redundant robotic arm telecontrol method based on inverse kinematics (its core employs the continuation method, abbreviated as CM, a perturbation and homotopy technique that combines unconstrained optimization methods, constrained optimization methods, and another numerical method), includes the following steps:

[0032] S1. Establish the kinematic model of the redundant robotic arm. For an n-DOF serial redundant robot (n≥6, m is the task space dimension, and m≤n), determine the target pose of the end effector in the m-dimensional task space. And the joint configuration vector q of the n-degree-of-freedom serial robotic arm, where m≤n, the positive kinematic function of the robotic arm is Φ(q), satisfying: ) ;

[0033] in, Let t be the pose vector in Cartesian space, t be the motion time, and Φ be the positive kinematics function of the serial robotic arm.

[0034] In the kinematic model of the redundant robotic arm described above, the pose of the actuator is described by a homogeneous transformation matrix, which consists of a rotation matrix and a translation vector. That is, Φ(q) can be represented as a homogeneous transformation matrix, derived from the rotation matrix. Translation vector Composition, specifically In this application, the terms "redundant robotic arm" and "redundant robot" are used interchangeably. A redundant robotic arm is generally a 7-DOF serial robotic arm, such as the Diana7, Iiwa14, Kinova Gen3, Franka Emika Panda, or a 7-DOF arm of a humanoid robot.

[0035] S2. Constructing the auxiliary extension function: First, define the objective function. The core error relationship characterizing the differential inverse kinematics problem is: ;

[0036] in To analyze the Jacobian matrix, For joint configuration velocity, For the end effector speed error, and , For the execution end to move from the current position p to the target position The linear velocity vector per unit time, For the execution end to change from the current attitude R to the target attitude The angular velocity vector per unit time.

[0037] Secondly, define the initial pose error function. , representing the deviation between the pose corresponding to the initial joint estimate and the target pose: ;

[0038] The initial joint estimate can be generated randomly, and the error between the initial joint and the target joint does not exceed the preset maximum allowable joint change η (the value of η ranges from 0.2 rad to 2.0 rad).

[0039] Finally, define the helper function. Establish the correlation between velocity error and position error: ,in, The derivative with respect to time and They exhibit a linear correlation, which improves the convergence stability of the function.

[0040] S3. Construct a fusion function based on the extension method, and use nonlinear homotopy parameters to construct the fusion function. This achieves a smooth transition from the auxiliary function to the objective function.

[0041] ;

[0042] Among them, homotopy parameter , This setting ensures that the value of s is always in the range [0,1], and that the extension path is smooth and differentiable; the fusion function satisfies the boundary conditions: and .

[0043] S4. Transform the inverse kinematics problem into a quadratic programming problem with joint velocity constraints, and define the objective function of the quadratic programming problem, with minimizing the fusion function error as the core:

[0044] ;

[0045] in, , These are the initial joint velocity estimates; the objective function can be further optimized to... , This is a regularization parameter (ranging from 0.001 to 0.1), used to improve numerical stability.

[0046] Then, define constraints to limit the joint speed within the allowable range: ;

[0047] in, This is the lower limit vector of joint velocity. This is the upper limit vector of joint velocity;

[0048] Lower limit of joint velocity for the i-th joint and The upper limits satisfy the following: , ;

[0049] in and These are the lower and upper limits of the range of motion of the i-th joint, respectively. and Let be the lower and upper limits of the angular velocity constraints for the i-th joint, respectively. The time interval (ranging from 0.001s to 0.01s).

[0050] S5. Iteratively solve and output the results: Solve the above quadratic programming problem using a quadratic programming solver, and set the inequality constraint matrix of the quadratic programming problem as follows (the inequality constraint matrix of the quadratic programming problem satisfies the following conditions):

[0051] ;

[0052] ;

[0053] in, The identity matrix is ​​of order n. Joint velocities are limited in real-time by constraint matrices A and B to avoid overshoot or joint impact. The quadratic programming solver uses the ProxQP solver (a near-end version of the Augmented Lagrange Method "ALM"), suitable for quadratic programming QP problems. The Jacobian matrix is ​​obtained through the Pinocchio Python interface (a wrapper in Python). All calculations use double-precision arithmetic.

[0054] Iteratively update the joint configuration vector q until the convergence condition is met. To preset the precision, output the final joint configuration vector.

[0055] Furthermore, during the iteration process, the actual pose of the end effector is updated in real time using a positive kinematics function, and the error between the actual pose and the target pose is calculated until the error meets the convergence condition or reaches the preset maximum number of iterations. The preset accuracy in the preferred convergence condition is selected. The initial value is set to It can be adjusted to a higher precision level as needed.

[0056] Specifically:

[0057] 1. Initialization parameters: Set the initial joint configuration. Time parameter t=0, iteration number i=0, parameter step size (Value range is 0.01-0.1), auxiliary function reinitialization interval (Values ​​range from 5 to 20 iterations), the initial pose is calculated using the positive kinematics function. , And calculate the initial error. , .

[0058] 2. Iterative solution: In each iteration, update the homotopy parameter. ,calculate And update the error The Jacobian matrix J is obtained through the Pinocchio interface, and the joint velocities are solved using the ProxQP quadratic programming solver. Then update the joint configuration, satisfying the following conditions during iterative update of the joint configuration vector: ,in , For unit time; recalculate the actual pose and error of the execution end.

[0059] 3. Convergence Criterion: When the convergence condition is met... ( To preset the accuracy, the initial value is generally set to... When the precision level can be adjusted to a higher level or when the preset maximum number of iterations (not less than 300 times) is reached, the iteration stops and the final joint configuration vector is output.

[0060] Alternatively, a damping effect can be introduced into the Jacobian matrix, and the Jacobian matrix can be corrected based on the Leweenberg-Marquardt method. , where k is the damping factor, I is the identity matrix, and is used to further improve the solution stability near singular points.

[0061] The above method is applied to a remotely operated robot system, where the communication frequency between the robot system and the external control device is no less than 500Hz, to achieve real-time motion control.

[0062] Furthermore, the solution efficiency of robot systems using this method is significantly improved. By integrating the extension method and quadratic programming, the convergence speed of the kinematic function is increased. Under the same solution accuracy and joint constraints, such as... Figure 1 As shown, the calculation speed is at least twice that of the Newton-Raphson method, the number of iterations is reduced by more than 50%, and the calculation time is stable within 2 milliseconds, which meets the requirements of real-time motion control.

[0063] It has strong stability and effectively solves the problems of traditional methods being sensitive to initial estimates and easily getting trapped in local minima by constructing a fusion function through nonlinear homotopy parameters. When the robot approaches the kinematic singularity, it can maintain the invertibility of the Jacobian matrix, avoid ill-conditioned solutions, and significantly reduce control deviation.

[0064] High solution accuracy, within the preset accuracy range At the same time, the accuracy rate exceeds 99.4%; under higher precision requirements, the accuracy rate can still be maintained above 96%, which is far superior to the traditional Newton-Raphson method and Lewinberg-Marquardt method.

[0065] It has wide applicability and can be used in various n-DOF redundant robots (especially 7-DOF redundant manipulators). It can handle multiple constraints such as joint range of motion and angular velocity, and can be applied to remotely operated robot systems, industrial robots, medical robots and other scenarios.

[0066] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims.

Claims

1. A redundant robotic arm remote control method based on inverse kinematics, characterized in that, Includes the following steps: S1. Establish the kinematic model of the redundant robotic arm and determine the target pose of the end effector in the m-dimensional task space. And the joint configuration vector q of the n-degree-of-freedom serial robotic arm, where m≤n, and the positive kinematic function of the robotic arm is Φ(q), satisfying ) , Let t be the pose vector in Cartesian space, t be the motion time, and Φ be the positive kinematics function of the serial robotic arm. S2. Construct auxiliary extension functions, including the objective function. Initial pose error function and auxiliary functions ,in To analyze the Jacobian matrix, For joint configuration velocity, For the end effector speed error, Initial estimates for the joints; S3. Constructing a fusion function based on the extension method Hotopism parameter , ; S4. Transform the inverse kinematics problem into a quadratic programming problem with joint velocity constraints, with the objective function being: The constraints are ,in , This is the lower limit vector of joint velocity. This is the upper limit vector of joint velocity; S5. Solve the above quadratic programming problem using a quadratic programming solver, iteratively updating the joint configuration vector q until the convergence condition is met. To preset the precision, output the final joint configuration vector.

2. The redundant robotic arm remote control method based on inverse kinematics according to claim 1, characterized in that, S4 Joint Velocity Lower Limit and The upper limits satisfy the following: , ; in and These are the lower and upper limits of the range of motion of the i-th joint, respectively. and Let be the lower and upper limits of the angular velocity constraints for the i-th joint, respectively. For time intervals.

3. The redundant robotic arm remote control method based on inverse kinematics according to claim 1 or 2, characterized in that, The inequality constraint matrix of the quadratic programming problem in S5 satisfies: , ,in It is an n-order identity matrix.

4. The redundant robotic arm remote control method based on inverse kinematics according to claim 3, characterized in that, Preset precision in convergence criteria The initial value is set to It can be adjusted to a higher precision level as needed.

5. The redundant robotic arm remote control method based on inverse kinematics according to claim 1, characterized in that, During the iteration process, the actual pose of the end effector is updated in real time through the positive kinematics function, and the error between the actual pose and the target pose is calculated until the error meets the convergence condition or reaches the preset maximum number of iterations.

6. The redundant robotic arm remote control method based on inverse kinematics according to claim 1, characterized in that, The quadratic programming solver uses the ProxQP solver, the Jacobian matrix is ​​obtained through the Pinocchio Python interface, and all calculations are performed using double-precision arithmetic.

7. The redundant robotic arm remote control method based on inverse kinematics according to claim 1, characterized in that, This also includes introducing a damping effect into the Jacobian matrix, and correcting the Jacobian matrix based on the Leweenberg-Marquardt method. , where k is the damping factor and I is the identity matrix.

8. The redundant robotic arm remote control method based on inverse kinematics according to claim 1, characterized in that, When iteratively updating the joint configuration vector, the following conditions must be met: ,in , Unit of time.

9. The redundant robotic arm remote control method based on inverse kinematics according to claim 1, characterized in that, In the kinematic model of the redundant robotic arm, the pose of the actuator is described by a homogeneous transformation matrix, which consists of a rotation matrix and a translation vector.

10. The redundant robotic arm remote control method based on inverse kinematics according to claim 1, characterized in that, The method is applied to a remotely operated robot system, where the communication frequency between the robot system and the external control device is no less than 500Hz, enabling real-time motion control.

Images