A method and device for real-time solution of inverse kinematics of a multi-degree-of-freedom robotic arm

Abstract

This invention discloses a method and apparatus for real-time solution of inverse kinematics of a multi-degree-of-freedom robotic arm. The method includes the following steps: establishing a forward kinematics model of the robotic arm to obtain the forward kinematics solution; defining a loss function and calculating the current pose error of the robotic arm in conjunction with the forward kinematics solution; calculating the gradient of the pose error when the current pose error exceeds a set error threshold; performing gradient normalization to prevent gradient explosion or vanishing; performing gradient filtering after gradient normalization to reduce noise interference with the solution results; adaptively adjusting the step size using a PID controller; setting and limiting the step size δ to prevent impact, updating joint variables, and ensuring real-time performance; obtaining the trajectory through calculation and performing smoothing filtering on the trajectory; recalculating the error until the set threshold condition is met. This invention, through an improved gradient descent algorithm, effectively improves the convergence speed and solution accuracy of inverse kinematics solution for robotic arms, and has robustness and applicability.

Description

Technical Field

[0001] This invention relates to the field of robot control and automation, specifically to a method and apparatus for real-time solving of the inverse kinematics of a multi-degree-of-freedom robotic arm based on an improved gradient descent algorithm. Background Technology

[0002] In industrial robot manufacturing, solving the inverse kinematics of the robotic arm is one of the key technologies for achieving precise trajectory control. However, traditional analytical methods are only applicable to a small number of robotic arms that satisfy the Pieper criterion, and their high computational complexity makes them difficult to apply to robotic arms with redundant degrees of freedom. To address this problem, some studies have employed intelligent optimization algorithms such as particle swarm optimization and butterfly optimization, but these methods suffer from high computational overhead and poor continuity of intermediate solutions during the solution process, limiting their application in real-time control.

[0003] In the field of machine learning, the introduction of the backpropagation (BP) algorithm has greatly advanced neural network algorithms, subsequently becoming a core algorithm in deep learning. The BP algorithm employs gradient descent during its backpropagation process, demonstrating its broad applicability in solving high-dimensional independent variables and its adaptability to various working scenarios, highlighting its strong flexibility. Therefore, how to apply the gradient descent algorithm to the real-time inverse kinematics solution of robotic arms is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0004] The purpose of this invention is to overcome the defect of poor continuity of intermediate solutions in the prior art, and to provide a method and device for real-time solution of inverse kinematics of multi-degree-of-freedom manipulators based on an improved gradient descent algorithm. This method and device can effectively improve the convergence speed and solution accuracy of real-time solution of inverse kinematics of manipulators, and has robustness and applicability.

[0005] To achieve the above objectives, this invention provides a method for real-time solution of inverse kinematics of a multi-degree-of-freedom robotic arm, comprising the following steps:

[0006] Step S1: Establish the forward kinematics model of the robotic arm to obtain the forward kinematics solution of the robotic arm;

[0007] Step S2: Define the loss function and calculate the current pose error of the robotic arm by combining the forward kinematics solution;

[0008] Step S3: When the current pose error is greater than the set error threshold, calculate the gradient of the pose error;

[0009] Step S4: Perform gradient normalization to prevent gradient explosion or vanishing.

[0010] Step S5: Perform gradient filtering after gradient normalization to reduce noise interference with the solution results;

[0011] Step S6: Adaptively adjust the step size using the PID controller;

[0012] Step S7: Set the step size δ and limit its value to prevent impact, update the joint variables, and ensure real-time performance;

[0013] Step S8: Calculate the trajectory and perform smoothing filtering on the trajectory;

[0014] Step S9: Recalculate the error until the set threshold condition is met.

[0015] Preferably, step S1 establishes a robotic arm model, and the pose of the robotic arm end can be obtained by the forward kinematics solution. In the process of solving the robotic arm pose, the first step is to establish a robot coordinate system. The DH parameter method uses many constraints to ensure the uniqueness of the robot coordinate system.

[0016] The pose matrix of each joint is obtained using the standard DH parameter method. The main logic is as follows: the pose of the robotic arm's end effector is calculated based on the joint parameters. The transformation of each joint is represented by a 4x4 homogeneous transformation matrix, where the pose transformation matrix between the i-th joint and the (i-1)-th joint is... for:

[0017]

[0018] By multiplying the pose transformation matrices of all joints sequentially, the formula for the forward kinematics solution of the robotic arm is obtained:

[0019]

[0020] The pose transformation matrix includes rotation and position information, and can be represented as:

[0021]

[0022] Where θ i The joint angle, i.e., the rotation angle, d i This refers to the link offset, specifically the translation along the previous joint axis, a. i The link length is α, which is the distance between the previous joint axis and the current joint axis. i is the link torsion angle, i.e., the torsion angle between the current joint axis and the previous joint axis; w represents the world coordinate system, represented by the identity matrix; and i represents the i-th joint of the robotic arm. R represents the pose transformation matrix of joint i relative to the world coordinate system w. A 3x3 rotation matrix describes the orientation of the robotic arm's end effector, and P represents the pose transformation matrix. A 3x1 position matrix describes the position of the robotic arm's end effector.

[0023] Preferably, in step S2, the loss function consists of position error and attitude error. Using the sum of position error and attitude error to track the target will result in faster convergence and reduce oscillations during the solution process, thus obtaining the error value.

[0024] The calculation logic for the position error includes: acquiring the current pose of the robotic arm's end effector in real time using the established robotic arm model; the target position is set by the user and is achieved by comparing the distance between the position of the robotic arm's end effector and the target position; and the position error formula L is calculated using the Pythagorean theorem. T :

[0025]

[0026] Among them, t x t y t z These represent the coordinates of the current position of the robotic arm's end effector, t' and t''. x ,t' y ,t' z These represent the coordinates of the target position of the robotic arm;

[0027] The attitude error L P Expressed using the 2-norm of a matrix, its formula is:

[0028] L P =||RR′||

[0029] Where R is the pose transformation matrix of the robotic arm's end effector. In the figure, R' is the rotation matrix and R' is the target pose transformation matrix of the robotic arm. The rotation matrix in the matrix.

[0030] Preferably, step S3 includes: calculating the pose error using a loss function; when the pose error is greater than a specified error value, calculating the gradient of the loss function; and obtaining the gradient vector expression of the loss function by calculating the partial derivatives of each joint parameter.

[0031]

[0032] Where X represents a high-dimensional independent variable, x i Represents the motion parameters of each joint, grad L (X) represents the gradient vector.

[0033] Preferably, step S4 normalizes the calculated gradient vector. The calculation logic is as follows: the magnitude of each component is adjusted to ensure that the sum of the squares of all gradient components is 1, thus preventing gradient vanishing or gradient explosion. For the gradient vector grad...L Each component g of (X) i The formula for calculating the sum of squares of the components is:

[0034]

[0035] Where g i It is the i-th component in the gradient vector, and n is the dimension of the gradient;

[0036] The normalization factor is used to adjust the gradient to a unit length. The normalization factor is the square root of the sum of the squares of the gradient components, denoted by the formula:

[0037]

[0038] Normalization is performed by dividing each component by a normalization factor, resulting in a normalized gradient grad. Ln Each component of (X) is represented by the following formula:

[0039]

[0040] Among them, grad L (X) represents the gradient vector, grad Ln (X) represents the normalized gradient vector;

[0041] Set the sum of squares of all gradient components to 1, and calculate using the following formula:

[0042]

[0043] Preferably, step S5 performs gradient filtering on the normalized gradient, including: receiving the normalized gradient vector grad at the current time. Ln (X), which represents the current gradient vector grad Ln (X) is combined with the smoothed gradient from the previous time step and smoothed using a weighted moving average filter, as shown in the formula:

[0044] grad Ln (X) filtered =ρ·grad Ln (X)+(1-ρ)·grad Ln (X) prev

[0045] Among them, grad Ln (X) filtered This represents the value after weighted gradient filtering, grad. Ln (X) prev This represents the smoothed gradient at the previous time step. ρ is the smoothing factor, with a value range of [0,1]. ρ is used to control the intensity of smoothing.

[0046] Preferably, the PID controller in step S6 represents proportional-integral-derivative control. Its calculation logic is as follows: obtain the error of the robotic arm's end effector reaching the target, and adjust the step size δ according to the magnitude of the error. The PID controller is based on PD control, and its step size adjustment range is limited by an exponential decay function. It adaptively adjusts the step size according to the error change, increasing the step size to accelerate the convergence speed when the error is large, and decreasing the step size to improve the calculation accuracy when the error is small. The calculation formula is as follows:

[0047]

[0048] Where P represents the proportional element in the PID controller, D represents the derivative element, and L... j δ represents the current j-th error. PD This indicates the step size after PD adjustment.

[0049] Preferably, step S7 includes: when switching targets, first setting the range of the next allowed step size δ, and controlling the angular velocity of each joint of the robotic arm by setting the step size, so as to respond quickly and update the joint variables in real time.

[0050] The range of step length δ and the joint variable update formula are as follows:

[0051]

[0052] θ j =θ j-1 -δ j ·grad Ln (X))

[0053] Where, δ j-1 This represents the step size value from the previous calculation. θ represents a constant coefficient that controls the proportion of the step size change. j Indicates the joint angle.

[0054] Preferably, in step S8, after gradient normalization, the change in each component in each iteration is less than 1. The change in each iteration depends on the step size. The step size is set through steps S6 and S7 and applied to the robotic arm to control the rate of change of each component. The intermediate solution generated in each iteration is used as the trajectory point of the robotic arm for real-time trajectory planning. During each iteration, the joint angle of the robotic arm is adjusted in real time using the current gradient direction until the error converges to a preset threshold or the maximum number of iterations is reached.

[0055] The trajectory filtering and smoothing process involves introducing a time constant to control the system's response speed to the input signal and iteratively calculating the output signal to effectively smooth out high-frequency noise components. The specific steps are as follows: A first-order inertial element transfer function is added for filtering; the inverse Laplace transform of the transfer function is performed to obtain its time-domain differential equation; the differential equation is discretized using the finite difference method, transforming it into a computer-processable form; the discretized equation is processed into a recursive formula for calculating its output; and the filtered signal is further filtered using a formula on the intermediate solutions.

[0056] The transfer function formula is as follows:

[0057]

[0058] Where C(s) and R(s) represent the Laplace transforms of the output and input signals, respectively, and T s It is the time constant, which controls the filter's response to high-frequency signals;

[0059] The differential equation is as follows:

[0060]

[0061] Where c(t) represents the output signal and r(t) represents the input signal;

[0062] The discretized differential equation is as follows:

[0063]

[0064] Where c(t)-c(t-dt) represents the derivative part that approximates the differential equation;

[0065] The calculated output c(t) recursive formula is as follows:

[0066]

[0067] This invention also provides a real-time inverse kinematics solving device for a multi-degree-of-freedom robotic arm, comprising:

[0068] The forward kinematics module is used to build the forward kinematics model of the robotic arm in order to obtain the forward kinematics solution of the robotic arm.

[0069] The loss module, which is connected to the forward kinematics module, is used to define the loss function and calculate the current pose error of the robotic arm by combining the forward kinematics solution.

[0070] The comparison module, which is connected to the loss module, is used to compare the current pose error with a set error threshold. If the current pose error is greater than the set error threshold, the gradient of the pose error is calculated. If the current pose error is less than or equal to the set error threshold, the calculated pose of the robotic arm end effector is output.

[0071] Gradient normalization module; the gradient normalization module is connected to the comparison module and is used to perform gradient normalization processing to prevent gradient explosion or vanishing.

[0072] Gradient filtering module; the gradient filtering module is connected to the gradient normalization module and is used to perform gradient filtering after gradient normalization to reduce noise interference on the solution results;

[0073] A PID control module; the PID control module is connected to the gradient filtering module and is used to adaptively adjust the step size through the PID controller; the step size δ is set and its value is limited to prevent shocks, and the joint variables are updated to ensure real-time performance;

[0074] The trajectory module is connected to the PID control module and is used to calculate the trajectory and perform smoothing filtering on the trajectory; and to recalculate the error until the set threshold condition is met.

[0075] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0076] 1. This invention is applicable to multi-degree-of-freedom robotic devices, including but not limited to flexible robotic arms, snake robots, and devices with multiple rotary joints. It is suitable for high-precision assembly tasks. This real-time solution method can be implemented on embedded hardware platforms, such as STM32 controllers, and is suitable for real-time control scenarios.

[0077] 2. This invention uses an improved gradient descent method to solve the inverse kinematics of a multi-freedom robotic arm in real time. By establishing a forward kinematics model of the robotic arm and defining a loss function, the pose error is obtained. Gradient normalization is used to prevent gradient vanishing when approaching the target pose, thus improving convergence accuracy. A PID controller is used to adaptively adjust the step size and step size limit to adjust the error changes, enabling real-time updates of the robotic arm's joint variable information and obtaining the real-time trajectory path. A weighted moving average filter and a first-order inertial filter are used to eliminate high-frequency jitter and reduce the error oscillation amplitude, improving the smoothness of the solution results. Attached Figure Description

[0078] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0079] Figure 1 This is a schematic diagram of a real-time inverse kinematics solution method for a multi-degree-of-freedom robotic arm provided by the present invention. Detailed Implementation

[0080] The technical solution of this embodiment of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiment is one embodiment of the present invention, and not all embodiments thereof. Based on this embodiment of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0081] Example 1

[0082] Reference Figure 1 As an embodiment of the present invention, a method for real-time solving of the inverse kinematics of a multi-degree-of-freedom manipulator based on an improved gradient descent algorithm is provided.

[0083] Step S1: Establish the forward kinematics model of the robotic arm to obtain the forward kinematics solution of the robotic arm.

[0084] In this embodiment, the solution to the forward kinematics of the robotic arm can be obtained by establishing a robotic arm model to determine the pose of the robotic arm's end effector. The first step in solving the robotic arm pose is to establish a robot coordinate system. The DH parameter method employs numerous constraints to ensure the uniqueness of the robot coordinate system. The DH standard parameter method simplifies the kinematic model, facilitates the expansion of robot models with different degrees of freedom, and standardizes the calculation expressions.

[0085] The pose matrix of each joint is obtained using the standard DH parameter method. The main logic is as follows: the pose of the robotic arm's end effector is calculated based on the joint parameters. The transformation of each joint is represented by a 4x4 homogeneous transformation matrix, where the pose transformation matrix between the i-th joint and the (i-1)-th joint is... for:

[0086]

[0087] By multiplying the pose transformation matrices of all joints sequentially, the formula for the forward kinematics solution of the robotic arm is obtained:

[0088]

[0089] The pose transformation matrix includes rotation and position information, and can be represented as:

[0090]

[0091] Where θ i The joint angle, i.e., the rotation angle, d i This refers to the link offset, specifically the translation along the previous joint axis, a. i The link length is α, which is the distance between the previous joint axis and the current joint axis. i is the link torsion angle, i.e., the torsion angle between the current joint axis and the previous joint axis; w represents the world coordinate system, represented by the identity matrix; and i represents the i-th joint of the robotic arm. R represents the pose transformation matrix of joint i relative to the world coordinate system w. A 3x3 rotation matrix describes the orientation of the robotic arm's end effector, and P represents the pose transformation matrix. A 3x1 position matrix describes the position of the robotic arm's end effector.

[0092] Step S2: Define the loss function and calculate the current pose error of the robotic arm by combining the positive kinematics solution.

[0093] In this embodiment, the loss function is defined as consisting of position error and attitude error. Using the sum of position and attitude errors to track the target leads to faster convergence and reduces oscillations during the solution process, thus obtaining the error value. By calculating the position error, the gradient descent algorithm can more effectively adjust the error feedback, helping the robotic arm's end effector quickly approach the target position. Optimizing in conjunction with attitude error improves the robotic arm's accuracy and convergence speed, especially in multi-degree-of-freedom systems, reducing oscillations and instability.

[0094] The calculation logic for the position error includes: acquiring the current pose of the robotic arm's end effector in real time using the established robotic arm model; the target position is set by the user and is achieved by comparing the distance between the position of the robotic arm's end effector and the target position; and the position error formula L is calculated using the Pythagorean theorem. T :

[0095]

[0096] Among them, t x t y t z These represent the coordinates of the current position of the robotic arm's end effector, t' and t''. x ,t' y ,t' zThese represent the coordinates of the target position of the robotic arm;

[0097] The attitude error L P Expressed using the 2-norm of a matrix, its formula is:

[0098] L P =||RR′‖

[0099] Where R is the pose transformation matrix of the robotic arm's end effector. In the figure, R' is the rotation matrix and R' is the target pose transformation matrix of the robotic arm. The rotation matrix in the matrix.

[0100] Step S3: When the current pose error is greater than the set error threshold, calculate the gradient of the pose error.

[0101] In this embodiment, the pose error is obtained by calculating a loss function, which includes: obtaining the pose error by calculating a loss function; when the pose error is greater than a specified error value, calculating the gradient of the loss function; and obtaining the gradient vector expression of the loss function by calculating the partial derivatives of each joint parameter.

[0102]

[0103]

[0104] Where X represents a high-dimensional independent variable, x i Represents the motion parameters of each joint, grad L (X) represents the gradient vector.

[0105] By calculating the partial derivatives of the gradient of the loss function and obtaining the gradient vector expression, the motion parameters of each joint of the robotic arm can be optimized. The gradient descent algorithm adjusts the motion parameters based on the gradient of the loss function, quickly finding the solution with the minimum error, thereby effectively reducing the motion error of the robotic arm and optimizing the performance of the control system.

[0106] Step S4: Perform gradient normalization to prevent gradient explosion or vanishing.

[0107] Step S4 normalizes the calculated gradient vector. Gradient normalization is performed to avoid the vanishing gradient problem, thereby improving the convergence speed of the gradient descent algorithm. In gradient descent, especially in deep neural networks, the vanishing gradient problem can cause the optimization process to be very slow or fail to converge. When the gradient becomes very small, the step size of parameter updates becomes extremely small, causing the optimization process to stall. The calculation logic is: adjust the magnitude of each component to ensure that the sum of the squares of all gradient components is 1, in order to prevent gradient vanishing or gradient explosion. L Each component g of (X) iThe formula for calculating the sum of squares of the components is:

[0108]

[0109] Where g i It is the i-th component in the gradient vector, and n is the dimension of the gradient;

[0110] The normalization factor is used to adjust the gradient to a unit length. The normalization factor is the square root of the sum of the squares of the gradient components, denoted by the formula:

[0111]

[0112] Normalization is performed by dividing each component by a normalization factor, resulting in a normalized gradient grad. Ln Each component of (X) is represented by the following formula:

[0113]

[0114] Among them, grad L (X) represents the gradient vector, grad Ln (X) represents the normalized gradient vector;

[0115] Set the sum of squares of all gradient components to 1, and calculate using the following formula:

[0116]

[0117] Step S5: Perform gradient filtering after gradient normalization to reduce noise interference with the solution results.

[0118] Step S5 performs gradient filtering on the normalized gradient, which can effectively reduce the interference of noise on the inverse kinematics solution of a multi-degree-of-freedom robotic arm and improve the calculation accuracy and stability; including: receiving the normalized gradient vector grad at the current time. Ln (X), which represents the current gradient vector grad Ln (X) is combined with the smoothed gradient from the previous time step and smoothed using a weighted moving average filter, as shown in the formula:

[0119] grad Ln (X) filtered =ρ·grad Ln (X)+(1-ρ)·grad Ln (X) prev

[0120] Among them, grad Ln (X) filtered This represents the value after weighted gradient filtering, grad. Ln (X)prev This represents the smoothed gradient at the previous time step. ρ is the smoothing factor, with a value range of [0,1]. ρ is used to control the intensity of smoothing.

[0121] Step S6: Adaptively adjust the step size using the PID controller.

[0122] The PID controller in step S6 represents proportional-integral-derivative control, used to adjust the step size based on the error L and its rate of change to smooth the movement of the robotic arm. During target switching, the error L may change drastically, therefore a smoothing strategy is needed to adjust the step size to avoid shocks caused by sudden error changes. Its calculation logic is as follows: obtain the error at which the robotic arm's end reaches the target, and adjust the step size δ according to the magnitude of the error. The PID controller is based on PD control, and its step size adjustment range is limited by an exponential decay function. It adaptively adjusts the step size according to the error changes, increasing the step size to accelerate convergence when the error is large and decreasing it to improve calculation accuracy when the error is small. This method can meet the control requirements of most linear systems and achieves good control performance through reasonable parameter adjustment. Its calculation formula is:

[0123]

[0124] Where P represents the proportional element in the PID controller, D represents the derivative element, and L... j δ represents the current j-th error. PD This indicates the step size after PD adjustment.

[0125] Step S7: Set the step size δ and limit its value to prevent impact, update the joint variables, and ensure real-time performance.

[0126] Step S7 includes: when the robotic arm switches targets, firstly set the range of the next allowed step size δ, and control the angular velocity of each joint of the robotic arm by setting the step size, so as to respond quickly and update the joint variables in real time; by smoothly transitioning the step size δ, the impact or vibration of the robotic arm caused by the sudden change in step size is avoided, thus improving the control stability of the robotic arm.

[0127] The range of step length δ and the joint variable update formula are as follows:

[0128]

[0129] θ j =θ j-1 -δ j ·grad Ln (X)

[0130] Where, δ j-1 This represents the step size value from the previous calculation. θ represents a constant coefficient that controls the proportion of the step size change. j Indicates the joint angle.

[0131] Step S8: Calculate the trajectory and perform smoothing filtering on the trajectory.

[0132] In step S8, after gradient normalization, the change in each component in each iteration is less than 1. The change in each iteration depends on the step size. The step size is set in steps S6 and S7 and applied to the robotic arm to control the rate of change of each component. The intermediate solutions generated in each iteration are used as the trajectory points of the robotic arm for real-time trajectory planning. During each iteration, the joint angles of the robotic arm are adjusted in real time using the current gradient direction until the error converges to a preset threshold or the maximum number of iterations is reached. Thus, the trajectory of the robotic arm during movement can be obtained in real time, and efficient and accurate trajectory planning can be performed to achieve more precise and faster movements.

[0133] The trajectory filtering and smoothing process involves introducing a time constant to control the system's response speed to the input signal and iteratively calculating the output signal to effectively smooth out high-frequency noise components. The specific steps are as follows: A first-order inertial element transfer function is added for filtering; the inverse Laplace transform of the transfer function is performed to obtain its time-domain differential equation; the differential equation is discretized using the finite difference method, transforming it into a computer-processable form; the discretized equation is processed into a recursive formula for calculating its output; and the filtered signal is further filtered using a formula on the intermediate solutions.

[0134] The transfer function formula is as follows:

[0135]

[0136] Where C(s) and R(s) represent the Laplace transforms of the output and input signals, respectively, and T s It is the time constant, which controls the filter's response to high-frequency signals;

[0137] The differential equation is as follows:

[0138]

[0139] Where c(t) represents the output signal and r(t) represents the input signal;

[0140] The discretized differential equation is as follows:

[0141]

[0142] Where c(t)-c(t-dt) represents the derivative part that approximates the differential equation;

[0143] The calculated output c(t) recursive formula is as follows:

[0144]

[0145] At each moment, the output signal c(t) depends not only on the current input r(t) but also on the output of the previous moment. This feedback mechanism enhances the "memory" of the input signal by increasing the time constant T, thereby smoothing out high-frequency components, making the output signal more stable, reducing the influence of high-frequency noise, and achieving the filtering effect.

[0146] Step S9: In this embodiment, the error is recalculated until the set threshold condition is met.

[0147] The method is applicable to multi-degree-of-freedom robotic devices, including but not limited to flexible robotic arms, snake robots, and devices with multiple rotary joints. It is suitable for high-precision assembly tasks and can be implemented on embedded hardware platforms, such as STM32 controllers, making it suitable for real-time control scenarios.

[0148] Through the above embodiments, the method of the present invention effectively solves the problems of high computational complexity in the solution process, difficulty in applying it to redundant degree-of-freedom robotic arms, poor continuity of intermediate solutions in the solution process, and difficulty in real-time control, which are problems that exist in the prior art. It significantly improves the performance requirements for solving key parameters in the field of robotics.

[0149] This invention employs an improved gradient descent method to solve the inverse kinematics of a multi-freedom robotic arm in real time. By establishing a forward kinematics model of the robotic arm and defining a loss function, the pose error is obtained. Gradient normalization prevents gradient vanishing as the target pose approaches, improving convergence accuracy. PID dynamic step size control and step size limits adaptively adjust to error changes, enabling real-time updates of joint variable information and obtaining the real-time trajectory path. A weighted moving average filter and a first-order inertial filter eliminate high-frequency jitter and reduce error oscillation amplitude, improving the smoothness of the solution results.

[0150] Example 2

[0151] Reference Figure 1 As one embodiment of the present invention, a real-time inverse kinematics solving device for a multi-degree-of-freedom robotic arm is provided, comprising:

[0152] The forward kinematics module is used to build the forward kinematics model of the robotic arm in order to obtain the forward kinematics solution of the robotic arm.

[0153] The loss module, which is connected to the forward kinematics module, is used to define the loss function and calculate the current pose error of the robotic arm by combining the forward kinematics solution.

[0154] The comparison module, which is connected to the loss module, is used to compare the current pose error with a set error threshold. If the current pose error is greater than the set error threshold, the gradient of the pose error is calculated. If the current pose error is less than or equal to the set error threshold, the calculated pose of the robotic arm end effector is output.

[0155] Gradient normalization module; the gradient normalization module is connected to the comparison module and is used to perform gradient normalization processing to prevent gradient explosion or vanishing.

[0156] Gradient filtering module; the gradient filtering module is connected to the gradient normalization module and is used to perform gradient filtering after gradient normalization to reduce noise interference on the solution results;

[0157] A PID control module; the PID control module is connected to the gradient filtering module and is used to adaptively adjust the step size through the PID controller; the step size δ is set and its value is limited to prevent shocks, and the joint variables are updated to ensure real-time performance;

[0158] The trajectory module is connected to the PID control module and is used to calculate the trajectory and perform smoothing filtering on the trajectory; and to recalculate the error until the set threshold condition is met.

[0159] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A method for real-time solution of inverse kinematics of a multi-degree-of-freedom robotic arm, characterized in that: Includes the following steps: Step S1: Establish the forward kinematics model of the robotic arm to obtain the forward kinematics solution of the robotic arm; Step S2: Define the loss function and calculate the current pose error of the robotic arm by combining the forward kinematics solution; Step S3: When the current pose error is greater than the set error threshold, calculate the gradient of the pose error; Step S4: Perform gradient normalization to prevent gradient explosion or vanishing. Step S5: Perform gradient filtering after gradient normalization to reduce noise interference with the solution results; Step S6: Adaptively adjust the step size using the PID controller; Step S7: Set the step size δ and limit its value to prevent impact, update the joint variables, and ensure real-time performance; Step S8: Calculate the trajectory and perform smoothing filtering on the trajectory; Step S9: Recalculate the error until the set threshold condition is met; In step S1, a robot arm model is established. The pose of the robot arm end can be obtained by the forward kinematics solution. In the process of solving the robot arm pose, the first step is to establish the robot coordinate system. The DH parameter method uses many constraints to ensure the uniqueness of the robot coordinate system. The pose matrix of each joint is obtained using the standard DH parameter method. The main logic is as follows: the pose of the robotic arm's end effector is calculated based on the joint parameters. The transformation of each joint is represented by a 4x4 homogeneous transformation matrix, where the pose transformation matrix between the i-th joint and the (i-1)-th joint is... for: By multiplying the pose transformation matrices of all joints sequentially, the formula for the forward kinematics solution of the robotic arm is obtained: The pose transformation matrix includes rotation and position information, and can be represented as: Where θ i The joint angle, i.e., the rotation angle, d i This refers to the link offset, specifically the translation along the previous joint axis, a. i The link length is α, which is the distance between the previous joint axis and the current joint axis. i is the link torsion angle, i.e., the torsion angle between the current joint axis and the previous joint axis; w represents the world coordinate system, represented by the identity matrix; and i represents the i-th joint of the robotic arm. R represents the pose transformation matrix of joint i relative to the world coordinate system w. A 3x3 rotation matrix describes the orientation of the robotic arm's end effector, and P represents the pose transformation matrix. A 3x1 position matrix describes the position of the robotic arm's end effector; In step S2, the loss function consists of position error and attitude error. Using the sum of position error and attitude error to track the target will result in faster convergence and reduce oscillations during the solution process, thus obtaining the error value. The calculation logic for the position error includes: acquiring the current pose of the robotic arm's end effector in real time using the established robotic arm model; the target position is set by the user and is achieved by comparing the distance between the robotic arm's end effector position and the target position; and deriving the position error formula L using the Pythagorean theorem. T : Among them, t x t y t z These represent the coordinates of the current position of the robotic arm's end effector, t' and t''. x ,t' y ,t' z These represent the coordinates of the target position of the robotic arm; The attitude error L P Expressed using the 2-norm of a matrix, its formula is: L P =||R-R′|| Where R is the pose transformation matrix of the robotic arm's end effector. In the figure, R' is the rotation matrix and R' is the target pose transformation matrix of the robotic arm. The rotation matrix in; Step S3 includes: calculating the pose error using a loss function; when the pose error is greater than a specified error value, calculating the gradient of the loss function; and obtaining the gradient vector expression of the loss function by calculating the partial derivatives of each joint parameter. Where X represents a high-dimensional independent variable, x i Represents the motion parameters of each joint, grad L (X) represents the gradient vector; Step S4 normalizes the calculated gradient vector. The calculation logic is as follows: the magnitude of each component is adjusted to ensure that the sum of the squares of all gradient components is 1, thus preventing gradient vanishing or gradient explosion. For the gradient vector grad... L Each component g of (X) i The formula for calculating the sum of squares of the components is: Where g i It is the i-th component in the gradient vector, and n is the dimension of the gradient; The normalization factor is used to adjust the gradient to a unit length. The normalization factor is the square root of the sum of the squares of the gradient components, denoted by the formula: Normalization is performed by dividing each component by a normalization factor, resulting in a normalized gradient grad. Ln Each component of (X) is represented by the following formula: Among them, grad L (X) represents the gradient vector, grad Ln (X) represents the normalized gradient vector; Set the sum of squares of all gradient components to 1, and calculate using the following formula: Step S5 performs gradient filtering on the normalized gradient, including: receiving the normalized gradient vector grad at the current time. Ln (X), which represents the current gradient vector grad Ln (X) is combined with the smoothed gradient from the previous time step and smoothed using a weighted moving average filter, as shown in the formula: degree Ln (X) filtered =ρ·grad Ln (X)+(1-ρ)·grad Ln (X) prev Among them, grad Ln (X) filtered This represents the value after weighted gradient filtering, grad. Ln (X) prev This represents the smoothed gradient at the previous time step. ρ is the smoothing factor, with a value range of [0,1]. ρ is used to control the intensity of smoothing.

2. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom robotic arm according to claim 1, characterized in that: The PID controller in step S6 represents proportional-integral-derivative control. Its calculation logic is as follows: It acquires the error at which the robotic arm's end-effector reaches the target, and adjusts the step size δ according to the magnitude of the error. The PID controller is based on PD control, and its step size adjustment range is limited by an exponential decay function. It adaptively adjusts the step size according to the error changes, increasing the step size to accelerate convergence when the error is large, and decreasing the step size to improve calculation accuracy when the error is small. The calculation formula is: Where P represents the proportional element in the PID controller, D represents the derivative element, and L... j δ represents the current j-th error. PD This indicates the step size after PD adjustment.

3. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom robotic arm according to claim 2, characterized in that: Step S7 includes: when switching targets, first set the range of the next allowed step size δ, and by setting the step size, control the angular velocity of each joint of the robotic arm to respond quickly and update the joint variables in real time. The range of step length δ and the joint variable update formula are as follows: i j =θ j-1 -d j ·grad Ln (X) Where, δ j-1 This represents the step size value from the previous calculation. θ represents a constant coefficient that controls the proportion of the step size change. j Indicates the joint angle.

4. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom robotic arm according to claim 3, characterized in that: In step S8, after gradient normalization, the change of each component in each iteration is less than 1. The change in each iteration depends on the step size. The step size is set in steps S6 and S7 and applied to the robotic arm to control the rate of change of each component. The intermediate solution generated in each iteration is used as the trajectory point of the robotic arm for real-time trajectory planning. During each iteration, the joint angle of the robotic arm is adjusted in real time using the current gradient direction until the error converges to the preset threshold or the maximum number of iterations is reached. The trajectory filtering and smoothing process involves introducing a time constant to control the system's response speed to the input signal and iteratively calculating the output signal to effectively smooth out high-frequency noise components. The specific steps are as follows: A first-order inertial element transfer function is added for filtering; the inverse Laplace transform of the transfer function is performed to obtain its time-domain differential equation; the differential equation is discretized using the finite difference method, transforming it into a computer-processable form; the discretized equation is processed into a recursive formula for calculating its output; and the filtered signal is further filtered using a formula on the intermediate solutions. The transfer function formula is as follows: Where C(s) and R(s) represent the Laplace transforms of the output and input signals, respectively, and T s It is the time constant, which controls the filter's response to high-frequency signals; The differential equation is as follows: Where c(t) represents the output signal and r(t) represents the input signal; The discretized differential equation is as follows: Where c(t)-c(t-dt) represents the derivative part that approximates the differential equation; The calculated output c(t) recursive formula is as follows:

5. A real-time inverse kinematics solving device for a multi-degree-of-freedom manipulator employing the real-time inverse kinematics solving method for a multi-degree-of-freedom manipulator as described in any one of claims 1 to 4, characterized in that, include: The forward kinematics module is used to build the forward kinematics model of the robotic arm in order to obtain the forward kinematics solution of the robotic arm. The loss module, which is connected to the forward kinematics module, is used to define the loss function and calculate the current pose error of the robotic arm by combining the forward kinematics solution. The comparison module, which is connected to the loss module, is used to compare the current pose error with a set error threshold. If the current pose error is greater than the set error threshold, the gradient of the pose error is calculated. If the current pose error is less than or equal to the set error threshold, the calculated pose of the robotic arm end effector is output. Gradient normalization module; the gradient normalization module is connected to the comparison module and is used to perform gradient normalization processing to prevent gradient explosion or vanishing. Gradient filtering module; the gradient filtering module is connected to the gradient normalization module and is used to perform gradient filtering after gradient normalization to reduce noise interference on the solution results; A PID control module; the PID control module is connected to the gradient filtering module and is used to adaptively adjust the step size through the PID controller; the step size δ is set and its value is limited to prevent shocks, and the joint variables are updated to ensure real-time performance; The trajectory module is connected to the PID control module and is used to calculate the trajectory and perform smoothing filtering on the trajectory; and to recalculate the error until the set threshold condition is met.

Images