Multi-degree-of-freedom mechanical arm inverse kinematics real-time solving method and device

Abstract

The invention discloses a multi-degree-of-freedom mechanical arm inverse kinematics real-time solving method and device, and the method comprises the following steps: building a forward kinematics model of a mechanical arm, so as to obtain a forward kinematics solution of the mechanical arm; defining a loss function, and calculating a current pose error of the mechanical arm in combination with a forward kinematics solution; when the current pose error is greater than a set error threshold value, calculating the gradient of the pose error; gradient normalization processing is carried out to prevent gradient explosion or disappearance; gradient filtering after gradient normalization is carried out, and interference of noise on a resolving result is reduced; the step length is adaptively adjusted through the PID controller; step length delta is set and limited, impact is prevented, joint variables are updated, and real-time performance is guaranteed; a track is obtained through calculation, and smooth filtering processing is carried out on the track; and recalculating the error until a set threshold condition is met. According to the method, through the improved gradient descent algorithm, the convergence speed and the calculation precision of inverse kinematics solution of the mechanical arm are effectively improved, and robustness and applicability are achieved.

Description

Technical Field

[0001] The present invention relates to the field of robot control and automation, and in particular to a method and device for real-time solving inverse kinematics of a multi-degree-of-freedom robot arm based on an improved gradient descent algorithm. Background Art

[0002] Solving the inverse kinematics of the manipulator in industrial robot production is one of the key technologies to achieve precise trajectory control. However, the traditional analytical method is only applicable to a few manipulators that meet the Pieper criterion, and the computational complexity is high, making it difficult to apply to manipulators with redundant degrees of freedom. To address this problem, some studies have used intelligent optimization algorithms such as particle swarm optimization and butterfly optimization, but these methods have problems such as high computational overhead and poor continuity of intermediate solutions during the solution process, which limits their application in real-time control.

[0003] In the field of machine learning, the introduction of the BP algorithm has greatly developed the neural network algorithm and has subsequently become the core algorithm of deep learning. The BP algorithm uses the gradient descent method in the back-propagation solution process, which shows that gradient descent has great universality in solving high-dimensional independent variables and can adapt to different work scenarios. It also shows that gradient descent has strong flexibility. Therefore, how to apply the gradient descent algorithm to the real-time solution of the inverse kinematics of the robot arm in the field of robotics is a problem that technicians in this field need to solve urgently. Summary of the invention

[0004] The purpose of the present 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, which 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 object, the present invention provides a real-time solution method for inverse kinematics of a multi-degree-of-freedom manipulator, comprising the following steps:

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

[0007] Step S2: define the loss function and calculate the current posture error of the robot arm in combination with the forward kinematics solution;

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

[0009] Step S4: Perform gradient normalization to prevent gradient explosion or disappearance;

[0010] Step S5: performing gradient filtering after gradient normalization to reduce the interference of noise on the solution result;

[0011] Step S6: adaptively adjusting the step size through a PID controller;

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

[0013] Step S8: Obtain the trajectory by calculation and perform smoothing filtering on the trajectory;

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

[0015] Preferably, the step S1 establishes a robot model, and the forward kinematics solution can obtain the posture of the end of the robot. In the process of solving the posture of the robot, the first step is to establish the robot coordinate system. Many constraints are used in the DH parameter method to ensure the uniqueness of the robot coordinate system.

[0016] The standard DH parameter method is used to obtain the pose matrix of each joint. Its main logic is: the pose of the end of the robot arm is calculated based on the joint parameters on the robot arm. The transformation of each joint is represented by a 4*4 homogeneous transformation matrix. 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 in sequence, the forward kinematics solution formula of the robot arm is obtained:

[0019]

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

[0021]

[0022] where θ i is the joint angle, i.e. the rotation angle, d i is the link offset, i.e. the translation along the previous joint axis, a i is the connecting rod length, that is, the distance between the previous joint axis and the current joint axis, α i is the joint torsion axis, that is, the angle between the current joint axis and the previous joint axis, w represents the world coordinate system, represented by the unit matrix, i represents the i-th joint of the robot arm, Represents the pose transformation matrix of joint i relative to the world coordinate system w, and R represents the pose transformation matrix A 3*3 rotation matrix in describes the orientation of the end of the robot arm, and P represents the pose transformation matrix A 3*1 position matrix in describes the position of the end of the robot arm.

[0023] Preferably, the loss function in step S2 is composed of a position error and a posture error. Using the sum of the position error and the posture error to track the target will allow the solution to converge more quickly and reduce oscillations during the solution process, thereby obtaining an error value;

[0024] The calculation logic of the position error includes: obtaining the current position of the end of the robot arm in real time through the established robot arm model, the target position is set by the user, and the distance between the position of the end of the robot arm and the target position is compared. The position error formula L is calculated by the Pythagorean theorem T :

[0025]

[0026] Among them, t x ,t y ,t z Represent the coordinates of the current position of the robot end position, t′ x , t′ y , t′ z Respectively represent the coordinates of the target position of the robot arm;

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

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

[0029] Among them, R is the end-arm pose transformation matrix The rotation matrix in , R′ is the robot target pose transformation matrix The rotation matrix in .

[0030] Preferably, step S3 includes: obtaining a posture error by calculating a loss function, and when the posture error is greater than a specified error value, calculating the gradient of the loss function, and obtaining a gradient vector expression of the loss function by performing partial derivative calculations on each joint parameter:

[0031]

[0032] Among them, 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, and its calculation logic is: adjust the size of each component to ensure that the sum of the squares of the gradient components is 1 to prevent the gradient from disappearing or exploding.L Each component g of (X) i , calculate the sum of the squares of the components, the formula is:

[0034]

[0035] where g i 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 unit length. The normalization factor is the square root of the sum of the squares of the gradient components, expressed as the formula:

[0037]

[0038] Normalization is performed by dividing each component by a normalization factor, so that the normalized gradient grad Ln Each component of (X) becomes 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 the squares of the gradient components to 1, and the calculation formula is:

[0042]

[0043] Preferably, the step S5 performs gradient filtering on the normalized gradient, including: receiving the normalized gradient vector grad Ln (X), the current gradient vector grad Ln (X) is combined with the smoothed gradient of the previous moment and smoothed by a weighted moving average filter, which is specifically expressed as the formula:

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

[0045] Among them, grad Ln (X) filtered Represents the value after weighted gradient filtering, grad Ln (X) prev Represents the smoothed gradient of the previous moment, ρ is the smoothing factor, and its value range is [0,1]. ρ is used to control the intensity of smoothing.

[0046] Preferably, the PID controller in step S6 represents proportional-integral-differential control, and its calculation logic is: obtain the error of the end of the robot arm reaching the target, and adjust the step length δ according to the error size. The PID controller is based on PD control, and its step length adjustment range is limited by an exponential decay function. The step length is adaptively adjusted according to the error change. When the error is large, the step length is increased to speed up the convergence speed, and when the error is small, the step length is reduced to improve the calculation accuracy. The calculation formula is:

[0047]

[0048] Among them, P represents the proportional link in PID, D represents the differential link in PID, and L j represents the current j-th error, δ PD Indicates the step size after PD adjustment.

[0049] Preferably, the step S7 comprises: when switching the target, firstly setting the range of values ​​of the step length δ allowed next time, and controlling the angular velocity of each joint of the robot arm by setting the step size, responding quickly, and updating the joint variables in real time;

[0050] The step length δ value range and joint variable update formula are:

[0051]

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

[0053] Among them, δ j-1 Indicates the step value of the last calculation, Represents a constant coefficient that controls the ratio of step size change, θ j Represents the joint angle.

[0054] Preferably, in step S8, after gradient normalization, the amount of change of each component in each iteration is less than 1, and the amount of change of each iteration depends on the step size. The step size is set by steps S6 and S7, and is applied to the robotic arm to control the change speed 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; in each iteration process, the current gradient direction is used to adjust the joint angle of the robotic arm in real time until the error converges to a preset threshold or reaches the maximum number of iterations;

[0055] The trajectory is filtered and smoothed by introducing a time constant to control the response speed of the system to the input signal, and the output signal is gradually calculated through an iterative formula to effectively smooth out the high-frequency noise component. The specific steps are: adding a first-order inertial link transfer function for filtering; performing an inverse Laplace transform on the transfer function to obtain its differential equation in the time domain; discretizing the differential equation by a difference method to convert it into a form that can be processed by a computer; processing the discretized equation into a recursive formula for calculating its output; and further filtering the intermediate solution of the filtered signal through a formula;

[0056] The transfer function formula:

[0057]

[0058] Where C(s) and R(s) represent the Laplace transform of the output and input signals respectively, T s is the time constant, controlling 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] Among them, c(t)-c(t-dt) represents the derivative part of the approximate differential equation;

[0065] The calculated output c(t) is recursively expressed as:

[0066]

[0067] The present invention also provides a real-time solution device for inverse kinematics of a multi-degree-of-freedom manipulator, comprising:

[0068] Forward kinematics module, used to establish the forward kinematics model of the robot arm to obtain the forward kinematics solution of the robot arm;

[0069] A loss module, which is connected to the forward kinematics module and is used to define a loss function and calculate the current posture error of the manipulator in combination with the forward kinematics solution;

[0070] A comparison module, which is connected to the loss module and is used to compare the current posture error with a set error threshold. If the current posture error is greater than the set error threshold, the gradient of the posture error is calculated; if the current posture error is less than or equal to the set error threshold, the posture of the end of the robot arm is calculated;

[0071] A 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 disappearance;

[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 the interference of noise on the solution result;

[0073] PID control module; the PID control module is connected to the gradient filter module and is used to adaptively adjust the step size through the PID controller; set the step size δ and limit its value to prevent impact, update the joint variables, and ensure real-time performance;

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

[0075] Compared with the prior art, the present invention has the following beneficial effects:

[0076] 1. The present invention is applicable to multi-degree-of-freedom robotic devices, including but not limited to flexible robotic arms, snake-like robots and devices with multiple rotational joints, and is applicable to high-precision assembly tasks. The real-time solution method can be implemented on an embedded hardware platform, such as an STM32 controller, and is applicable to real-time control scenarios.

[0077] 2. The present invention uses an improved gradient descent method to perform real-time inverse kinematics solution of a multi-freedom robotic arm, and obtains the posture error by establishing a forward kinematics model of the robotic arm and defining a loss function. By gradient normalization, the gradient vanishing is prevented when approaching the target posture, thereby improving the convergence accuracy. The error change is adaptively adjusted by the PID controller through adaptive adjustment of the step size and step size limit, thereby realizing real-time update of the robotic arm joint variable information and obtaining the real-time trajectory path. By using a weighted sliding average filter and a first-order inertial link filter, high-frequency jitter is eliminated and the error oscillation amplitude is reduced, thereby improving the smoothness of the solution result. BRIEF DESCRIPTION OF THE DRAWINGS

[0078] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0079] Figure 1 It is a schematic diagram of a real-time solution method for inverse kinematics of a multi-degree-of-freedom manipulator provided by the present invention. DETAILED DESCRIPTION

[0080] The technical scheme in this embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in this embodiment of the present invention. Obviously, the described embodiment is one embodiment of the present invention, not all embodiments of the present invention. Based on this embodiment of the present invention, all other embodiments of the present invention obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0081] Embodiment 1

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

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

[0084] In this embodiment, the solution of the forward kinematics of the robot arm can be obtained by establishing a robot arm model to obtain the posture of the end of the robot arm. In the process of solving the posture of the robot arm, the first step is to establish the robot coordinate system. Many constraints are used in the DH parameter method to ensure the uniqueness of the robot coordinate system. Through the DH standard parameter method, the kinematic model can be simplified, which is convenient for the expansion of robot models with different degrees of freedom and standardizes the calculation expression.

[0085] The standard DH parameter method is used to obtain the pose matrix of each joint. Its main logic is: the pose of the end of the robot arm is calculated based on the joint parameters on the robot arm. The transformation of each joint is represented by a 4*4 homogeneous transformation matrix. 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 in sequence, the forward kinematics solution formula of the robot arm is obtained:

[0088]

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

[0090]

[0091] where θ i is the joint angle, i.e. the rotation angle, d i is the link offset, i.e. the translation along the previous joint axis, a i is the connecting rod length, that is, the distance between the previous joint axis and the current joint axis, α i is the joint torsion axis, that is, the angle between the current joint axis and the previous joint axis, w represents the world coordinate system, represented by the unit matrix, i represents the i-th joint of the robot arm, Represents the pose transformation matrix of joint i relative to the world coordinate system w, and R represents the pose transformation matrix A 3*3 rotation matrix in describes the orientation of the end of the robot arm, and P represents the pose transformation matrix A 3*1 position matrix in describes the position of the end of the robot arm.

[0092] Step S2: Define the loss function and calculate the current posture error of the robot arm in combination with the forward kinematics solution.

[0093] In this embodiment, the loss function is defined, where the loss function is composed of position error and attitude error. The sum of position error and attitude error is used to track the target, and the solution converges more quickly and can reduce the oscillation in the solution process, thereby obtaining the error value. Through the calculation of the position error, the gradient descent algorithm can more effectively perform error feedback adjustment to help the end of the robot arm quickly approach the target position. Optimization combined with attitude error can improve the accuracy and convergence speed of the robot arm, especially when dealing with multi-degree-of-freedom systems, which can reduce oscillation and instability.

[0094] The calculation logic of the position error includes: obtaining the current position of the end of the robot arm in real time through the established robot arm model, the target position is set by the user, and the distance between the position of the end of the robot arm and the target position is compared. The position error formula L is calculated by the Pythagorean theorem T :

[0095]

[0096] Among them, t x ,t y ,t z Represent the coordinates of the current position of the robot end position, t′ x , t′ y , t′ zRespectively represent the coordinates of the target position of the robot arm;

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

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

[0099] Among them, R is the end-arm pose transformation matrix The rotation matrix in , R′ is the robot target pose transformation matrix The rotation matrix in .

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

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

[0102]

[0103]

[0104] Among them, 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 derivative of the gradient of the loss function and obtaining the gradient vector expression, the motion parameters of each joint of the robot can be optimized. The gradient descent algorithm adjusts the motion parameters based on the gradient of the loss function and can quickly find the solution with the minimum error, thereby effectively reducing the motion error of the robot and optimizing the performance of the control system.

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

[0107] The step S4 normalizes the calculated gradient vector. Gradient normalization is to avoid the gradient vanishing problem, thereby improving the convergence speed of the gradient descent algorithm. In gradient descent, especially in deep neural networks, the gradient vanishing problem can cause the optimization process to be very slow or unable to converge. When the gradient becomes very small, the step size of the parameter update will become extremely small, causing the optimization process to stagnate. The calculation logic is: adjust the size of each component to ensure that the sum of the squares of the gradient components is 1 to prevent the gradient from vanishing or exploding. For the gradient vector grad L Each component g of (X)i , calculate the sum of the squares of the components, the formula is:

[0108]

[0109] where g i 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 unit length. The normalization factor is the square root of the sum of the squares of the gradient components, expressed as the formula:

[0111]

[0112] Normalization is performed by dividing each component by a normalization factor, so that the normalized gradient grad Ln Each component of (X) becomes 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 the squares of the gradient components to 1, and the calculation formula is:

[0116]

[0117] Step S5: Perform gradient filtering after gradient normalization to reduce the interference of noise on the solution result.

[0118] The step S5 performs gradient filtering on the normalized gradient, which can effectively reduce the interference of noise on the inverse kinematics solution of the multi-degree-of-freedom manipulator and improve the calculation accuracy and stability; comprising: receiving the normalized gradient vector grad Ln (X), the current gradient vector grad Ln (X) is combined with the smoothed gradient of the previous moment and smoothed by a weighted moving average filter, which is specifically expressed as the formula:

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

[0120] Among them, grad Ln (X) filtered Represents the value after weighted gradient filtering, grad Ln(X) prev Represents the smoothed gradient of the previous moment, ρ is the smoothing factor, and its value range is [0,1]. ρ is used to control the intensity of smoothing.

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

[0122] The PID controller of step S6 represents proportional-integral-differential control, which is used to adjust the step length according to the error L and the rate of change of the error to smooth the movement of the robot arm. When the target is switched, the error L may change dramatically, so it is necessary to use a smoothing strategy to adjust the change of the step length to avoid the impact caused by the sudden change of the error; its calculation logic is: obtain the error of the end of the robot arm reaching the target, and adjust the step length δ according to the error size. The PID controller is based on PD control, and its step length adjustment range is limited by an exponential decay function. The step length is adaptively adjusted according to the error change. When the error is large, the step length is increased to speed up the convergence speed, and when the error is small, the step length is reduced to improve the calculation accuracy. This method can meet the control requirements of most linear systems and obtain better control effects by reasonably adjusting parameters; its calculation formula is:

[0123]

[0124] Among them, P represents the proportional link in PID, D represents the differential link in PID, and L j represents the current j-th error, δ PD Indicates the step size after PD adjustment.

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

[0126] The step S7 comprises: when the robot arm switches the target, firstly setting the range of the step length δ allowed for the next time, and by setting the step length, controlling the angular velocity of each joint of the robot arm, responding quickly, and updating the joint variables in real time; by smoothly transitioning the step length δ, avoiding the impact or vibration of the robot arm caused by the sudden change of the step length, and improving the control stability of the robot arm;

[0127] The step length δ value range and joint variable update formula are:

[0128]

[0129] θ j =θ j-1 -δ j ·gradL n (X)

[0130] Among them, δ j-1 Indicates the step value of the last calculation, Represents a constant coefficient that controls the ratio of step size change, θ j Represents the joint angle.

[0131] Step S8: Obtain the trajectory through calculation and perform smoothing filtering on the trajectory.

[0132] In step S8, after the gradient is normalized, the change of each component in each iteration is less than 1, and the change of 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 change speed 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; in each iteration process, the current gradient direction is used to adjust the joint angle of the robotic arm in real time until the error converges to a preset threshold or reaches the maximum number of iterations; at this point, 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 rapid movements.

[0133] The trajectory is filtered and smoothed by introducing a time constant to control the response speed of the system to the input signal, and the output signal is gradually calculated through an iterative formula to effectively smooth out the high-frequency noise component. The specific steps are: adding a first-order inertial link transfer function for filtering; performing an inverse Laplace transform on the transfer function to obtain its differential equation in the time domain; discretizing the differential equation by a difference method to convert it into a form that can be processed by a computer; processing the discretized equation into a recursive formula for calculating its output; and further filtering the intermediate solution of the filtered signal through a formula;

[0134] The transfer function formula:

[0135]

[0136] Where C(s) and R(s) represent the Laplace transform of the output and input signals respectively, T s is the time constant, controlling 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] Among them, c(t)-c(t-dt) represents the derivative part of the approximate differential equation;

[0143] The calculated output c(t) is recursively expressed as:

[0144]

[0145] At each moment, the output signal c(t) depends not only on the current input r(t), but is also affected by the output at the previous moment. This feedback mechanism enhances the "memory" of the input signal by increasing the time constant T, thereby smoothing out the high-frequency components, making the output signal more stable, and reducing the impact of high-frequency noise to achieve a 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-like robots, and devices with multiple rotational joints, and is applicable to high-precision assembly tasks. The method can be implemented on embedded hardware platforms, such as STM32 controllers, and is applicable to real-time control scenarios;

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

[0149] The present invention uses an improved gradient descent method to perform real-time inverse kinematics solutions for a multi-freedom manipulator, and obtains the posture error by establishing a forward kinematics model of the manipulator and defining a loss function. Gradient normalization is used to prevent gradient vanishing when approaching the target posture, thereby improving convergence accuracy. PID dynamic step control and step limit are used to adaptively adjust the error change, achieve real-time updating of the manipulator joint variable information, and obtain the real-time trajectory path. A weighted sliding average filter and a first-order inertial link filter are used to eliminate high-frequency jitter and reduce the error oscillation amplitude, thereby improving the smoothness of the solution result.

[0150] Embodiment 2

[0151] Reference Figure 1 According to an embodiment of the present invention, a real-time inverse kinematics solution device for a multi-degree-of-freedom manipulator is provided, comprising:

[0152] Forward kinematics module, used to establish the forward kinematics model of the robot arm to obtain the forward kinematics solution of the robot arm;

[0153] A loss module, which is connected to the forward kinematics module and is used to define a loss function and calculate the current posture error of the manipulator in combination with the forward kinematics solution;

[0154] A comparison module, which is connected to the loss module and is used to compare the current posture error with a set error threshold. If the current posture error is greater than the set error threshold, the gradient of the posture error is calculated; if the current posture error is less than or equal to the set error threshold, the posture of the end of the robot arm is calculated;

[0155] A 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 disappearance;

[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 the interference of noise on the solution result;

[0157] PID control module; the PID control module is connected to the gradient filter module and is used to adaptively adjust the step size through the PID controller; set the step size δ and limit its value to prevent impact, update the joint variables, and ensure real-time performance;

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

[0159] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the protection scope of the present invention.

Claims

1. A real-time solution method for inverse kinematics of a multi-degree-of-freedom manipulator, characterized by: The following steps are involved: Step S1: Establish a forward kinematics model of the robot arm to obtain a forward kinematics solution of the robot arm; Step S2: define the loss function and calculate the current posture error of the robot arm in combination with the forward kinematics solution; Step S3: When the current posture error is greater than the set error threshold, the gradient of the posture error is calculated; Step S4: Perform gradient normalization to prevent gradient explosion or disappearance; Step S5: performing gradient filtering after gradient normalization to reduce the interference of noise on the solution result; Step S6: adaptively adjusting the step size through a PID controller; Step S7: Set the step length δ and limit its value to prevent impact, update joint variables, and ensure real-time performance; Step S8: Obtain the trajectory by calculation and perform smoothing filtering on the trajectory; Step S9: Recalculate the error until the set threshold condition is met.

2. A real-time solution method for inverse kinematics of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: The step S1 establishes a robot model. The forward kinematics solution can be used to obtain the position and posture of the end of the robot. In the process of solving the position and posture of the robot, the first step is to establish the robot coordinate system. Many constraints are used in the DH parameter method to ensure the uniqueness of the robot coordinate system. The standard DH parameter method is used to obtain the pose matrix of each joint. Its main logic is: the pose of the end of the robot arm is calculated based on the joint parameters on the robot arm. The transformation of each joint is represented by a 4*4 homogeneous transformation matrix. 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 in sequence, the forward kinematics solution formula of the robot arm is obtained: The pose transformation matrix includes rotation and position information, which can be expressed as: where θ i is the joint angle, i.e. the rotation angle, d i is the link offset, i.e. the translation along the previous joint axis, a i is the connecting rod length, that is, the distance between the previous joint axis and the current joint axis, α i is the joint torsion axis, that is, the angle between the current joint axis and the previous joint axis, w represents the world coordinate system, represented by the unit matrix, i represents the i-th joint of the robot arm, Represents the pose transformation matrix of joint i relative to the world coordinate system w, and R represents the pose transformation matrix A 3*3 rotation matrix in describes the orientation of the end of the robot arm, and P represents the pose transformation matrix A 3*1 position matrix in describes the position of the end of the robot arm.

3. A real-time solution method for inverse kinematics of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: In step S2, the loss function is composed of a position error and a posture error. Using the sum of the position error and the posture error to track the target will allow the solution to converge more quickly and reduce the oscillation during the solution process, thereby obtaining an error value. The calculation logic of the position error includes: obtaining the current position of the end of the robot arm in real time through the established robot arm model, the target position is set by the user, and the distance between the position of the end of the robot arm and the target position is compared. The position error formula L is calculated by the Pythagorean theorem T : Among them, t x ,t y ,t z Represent the coordinates of the current position of the robot end position, t′ x , t′ y , t′ z Respectively represent the coordinates of the target position of the robot arm; The attitude error L P Expressed using the 2-norm of the matrix, the formula is: L P =||R-R′|| Among them, R is the end-arm pose transformation matrix The rotation matrix in, R′ is the robot target pose transformation matrix The rotation matrix in .

4. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: The step S3 comprises: obtaining the posture error by calculating the loss function, and when the posture error is greater than the specified error value, calculating the gradient of the loss function, and obtaining the gradient vector expression of the loss function by calculating the partial derivative of each joint parameter: Among them, X represents a high-dimensional independent variable, x i Represents the motion parameters of each joint, grad L (X) represents the gradient vector.

5. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: The step S4 normalizes the calculated gradient vector. The calculation logic is: adjust the size of each component to ensure that the sum of the squares of the gradient components is 1 to prevent the gradient from disappearing or exploding. L Each component g of (X) i , calculate the sum of the squares of the components, the formula is: where g i 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 unit length. The normalization factor is the square root of the sum of the squares of the gradient components, expressed as the formula: Normalization is performed by dividing each component by a normalization factor, so that the normalized gradient grad Ln Each component of (X) becomes the following formula: Among them, grad L (X) represents the gradient vector, grad Ln (X) represents the normalized gradient vector; Set the sum of the squares of the gradient components to 1, and the calculation formula is:

6. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: The step S5 performs gradient filtering on the normalized gradient, including: receiving the normalized gradient vector grad Ln (X), the current gradient vector grad Ln (X) is combined with the smoothed gradient of the previous moment and smoothed by a weighted moving average filter, which is specifically expressed as the formula: degree Ln (X) filtered =ρ·grad Ln (X)+(1-ρ)·grad Ln (X) prev Among them, grad Ln (X) filtered Represents the value after weighted gradient filtering, grad Ln (X) prev Represents the smoothed gradient of the previous moment, ρ is the smoothing factor, and its value range is [0,1]. ρ is used to control the intensity of smoothing.

7. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: The PID controller in step S6 represents proportional-integral-differential control, and its calculation logic is: obtain the error of the end of the robot arm reaching the target, and adjust the step length δ according to the error size. The PID controller is based on PD control, and its step length adjustment range is limited by an exponential decay function. The step length is adaptively adjusted according to the error change. When the error is large, the step length is increased to speed up the convergence speed, and when the error is small, the step length is reduced to improve the calculation accuracy. The calculation formula is: Among them, P represents the proportional link in PID, D represents the differential link in PID, and L j represents the current j-th error, δ PD Indicates the step size after PD adjustment.

8. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: The step S7 comprises: when switching the target, firstly setting the range of the step length δ allowed next time, and by setting the step length, controlling the angular velocity of each joint of the robot arm, responding quickly, and updating the joint variables in real time; The step length δ value range and joint variable update formula are: i j =θ j-1 -d j ·grad Ln (X) Among them, δ j-1 Indicates the step value of the last calculation, Represents a constant coefficient that controls the ratio of step size change, θ j Represents the joint angle.

9. The method for real-time solution of inverse kinematics of a multi-degree-of-freedom manipulator according to claim 1, characterized in that: In step S8, after the gradient is normalized, the change amount of each component in each iteration is less than 1, and the change amount of each iteration depends on the step size. The step size is set through steps S6 and S7, and is applied to the robotic arm to control the change speed 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; in each iteration process, the current gradient direction is used to adjust the joint angle of the robotic arm in real time until the error converges to a preset threshold or reaches the maximum number of iterations; The trajectory is filtered and smoothed by introducing a time constant to control the response speed of the system to the input signal, and the output signal is gradually calculated through an iterative formula to effectively smooth out the high-frequency noise component. The specific steps are: adding a first-order inertial link transfer function for filtering; performing an inverse Laplace transform on the transfer function to obtain its differential equation in the time domain; discretizing the differential equation by a difference method to convert it into a form that can be processed by a computer; processing the discretized equation into a recursive formula for calculating its output; and further filtering the intermediate solution of the filtered signal through a formula; The transfer function formula: Where C(s) and R(s) represent the Laplace transform of the output and input signals respectively, T s is the time constant, controlling 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: Among them, c(t)-c(t-dt) represents the derivative part of the approximate differential equation; The calculated output c(t) is recursively expressed as:

10. A real-time inverse kinematics solution device for a multi-degree-of-freedom manipulator, characterized in that: include: Forward kinematics module, used to establish the forward kinematics model of the robot arm to obtain the forward kinematics solution of the robot arm; A loss module, which is connected to the forward kinematics module and is used to define a loss function and calculate the current posture error of the manipulator in combination with the forward kinematics solution; A comparison module, which is connected to the loss module and is used to compare the current posture error with a set error threshold. If the current posture error is greater than the set error threshold, the gradient of the posture error is calculated; if the current posture error is less than or equal to the set error threshold, the posture of the end of the robot arm is calculated; A 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 disappearance; 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 the interference of noise on the solution result; PID control module; the PID control module is connected to the gradient filter module and is used to adaptively adjust the step size through the PID controller; set the step size δ and limit its value to prevent impact, update the joint variables, and ensure real-time performance; The trajectory module is connected to the PID control module and is used to obtain the trajectory by calculation and perform smoothing filtering on the trajectory; and recalculate the error until the set threshold condition is met.

Images