Method, device and equipment for avoiding joint limitation of redundant degree of freedom robot

Abstract

The present application relates to a method, device and equipment for avoiding joint limitation of a redundant degree of freedom robot, wherein the method includes: obtaining the current joint angle and target posture of the robot; moving the limited joint in the robot joint in a preset range of motion in a direction away from the limit, and constructing a dynamic Jacobian matrix based on the adjusted joint angle; obtaining the initial posture of the Cartesian space corresponding to the current joint angle, comparing the initial posture and the target posture to obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculating the pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix; calculating the joint increment based on the deviation vector and the pseudo-inverse Jacobian matrix; updating the current joint angle based on the joint increment, and re-using the updated joint angle as the current joint angle, returning to the step of obtaining the initial posture of the Cartesian space corresponding to the current joint angle, until the latest deviation vector obtained is less than the preset error threshold, and obtaining the target joint angle.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and in particular to a method, device, computer equipment, storage medium, and computer program product for avoiding joint limitation of a redundant-freedom robot. Background Art

[0002] With the rapid development of automation and robotics technologies, redundant robotic arms have demonstrated tremendous potential and value in various advanced automation and robotics applications. As the name suggests, redundant robotic arms have more degrees of freedom than are necessary to complete a specific task. This design gives the robotic arm greater flexibility and a wider range of operational capabilities.

[0003] In applications such as welding and gluing, robots are often required to precisely track a specific task path. These tasks require the robot to perform tasks without interruption and must complete the entire task path in one go. However, in practical applications, especially when the task path is long or complex, even robots with redundant degrees of freedom may face joint limitations. Joint limitations occur when one or more joints of a robot reach their mechanical or physical limits and are no longer able to move normally, often leading to task failure.

[0004] Therefore, there is an urgent need for an effective redundant degree of freedom robot joint limitation avoidance solution to prevent the robot from being unable to continue normal movement due to joint limitations. Summary of the Invention

[0005] Based on this, it is necessary to provide an effective redundant freedom robot joint limitation avoidance method, device, computer equipment, computer readable storage medium and computer program product to address the above technical problems.

[0006] In a first aspect, the present application provides a method for avoiding joint limitation of a redundant degree of freedom robot. The method comprises:

[0007] Get the robot's current joint angle and target pose;

[0008] Move the limited joints in the robot joints in a preset range of motion in a direction away from the limit to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle;

[0009] Obtaining an initial pose in Cartesian space corresponding to the current joint angle, comparing the initial pose with the target pose to obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculating a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix;

[0010] Calculating a joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix;

[0011] The current joint angle is updated based on the joint increment, and the updated joint angle is used as the current joint angle again, and the step of obtaining the initial posture in the Cartesian space corresponding to the current joint angle is returned until the latest deviation vector is less than the preset error threshold to obtain the target joint angle.

[0012] In one embodiment, moving a limited joint in a robot joint in a direction away from the limit by a preset range of motion to obtain a new joint angle, and constructing a dynamic Jacobian matrix based on the adjusted joint angle includes:

[0013] Move the limited joints in the robot joints away from the limit position in the preset range of motion to obtain a new joint angle;

[0014] A Jacobian matrix is ​​constructed based on the new joint angles, and the column elements corresponding to the limit joints in the constructed Jacobian matrix are eliminated to obtain a dynamic Jacobian matrix.

[0015] In one embodiment, comparing the initial pose and the target pose to obtain a deviation vector includes:

[0016] Comparing the initial posture and the target posture, and calculating the position deviation and the posture deviation;

[0017] The position deviation and attitude deviation are combined to obtain the deviation vector.

[0018] In one embodiment, comparing the initial posture and the target posture to calculate the position deviation and the posture deviation includes:

[0019] Subtracting the position portion of the initial posture from the target posture to obtain a position deviation;

[0020] Calculating the inverse matrix product of the initial pose and the pose portion of the target pose;

[0021] The inverse matrix product is converted into an axis-angle form to obtain the attitude deviation.

[0022] In one embodiment, obtaining the initial pose in Cartesian space corresponding to the current joint angle includes:

[0023] Substitute the current joint angle into the robot's forward kinematics to calculate the robot's initial Cartesian space pose.

[0024] In one embodiment, the predetermined range of motion comprises 5% of the overall range of motion of the joint.

[0025] In a second aspect, the present application also provides a redundant degree of freedom robot joint limit avoidance device. The device comprises:

[0026] Parameter acquisition module, used to obtain the robot's current joint angle and target posture;

[0027] A fine-tuning module is used to move the limited joints in the robot's joints in a preset range of motion in a direction away from the limit to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle;

[0028] a deviation calculation module, configured to obtain an initial Cartesian space pose corresponding to the current joint angle, compare the initial pose with the target pose, obtain a deviation vector, and calculate a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix if the deviation vector is not less than a preset error threshold;

[0029] an increment calculation module, configured to calculate a joint increment based on the deviation vector and the pseudo-inverse Jacobian matrix;

[0030] An iterative adjustment module is used to update the current joint angle based on the joint increment, and to use the updated joint angle as the current joint angle, and to control the deviation calculation module to re-execute the operation of obtaining the initial posture in the Cartesian space corresponding to the current joint angle until the latest deviation vector is less than the preset error threshold, thereby obtaining the target joint angle.

[0031] In a third aspect, the present application further provides a computer device. The computer device includes a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, the following steps are performed:

[0032] Get the robot's current joint angle and target pose;

[0033] Move the limited joints in the robot joints in a preset range of motion in a direction away from the limit to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle;

[0034] Obtaining an initial pose in Cartesian space corresponding to the current joint angle, comparing the initial pose with the target pose to obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculating a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix;

[0035] Calculating a joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix;

[0036] The current joint angle is updated based on the joint increment, and the updated joint angle is used as the current joint angle again, and the step of obtaining the initial posture in the Cartesian space corresponding to the current joint angle is returned until the latest deviation vector is less than the preset error threshold to obtain the target joint angle.

[0037] In a fourth aspect, the present application further provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the following steps:

[0038] Get the robot's current joint angle and target pose;

[0039] Move the limited joints in the robot joints in a preset range of motion in a direction away from the limit to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle;

[0040] Obtaining an initial pose in Cartesian space corresponding to the current joint angle, comparing the initial pose with the target pose to obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculating a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix;

[0041] Calculating a joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix;

[0042] The current joint angle is updated based on the joint increment, and the updated joint angle is used as the current joint angle again, and the step of obtaining the initial posture in the Cartesian space corresponding to the current joint angle is returned until the latest deviation vector is less than the preset error threshold to obtain the target joint angle.

[0043] In a fifth aspect, the present application further provides a computer program product. The computer program product includes a computer program that, when executed by a processor, implements the following steps:

[0044] Get the robot's current joint angle and target pose;

[0045] Move the limited joints in the robot joints in a preset range of motion in a direction away from the limit to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle;

[0046] Obtaining an initial pose in Cartesian space corresponding to the current joint angle, comparing the initial pose with the target pose to obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculating a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix;

[0047] Calculating a joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix;

[0048] The current joint angle is updated based on the joint increment, and the updated joint angle is used as the current joint angle again, and the step of obtaining the initial posture in the Cartesian space corresponding to the current joint angle is returned until the latest deviation vector is less than the preset error threshold to obtain the target joint angle.

[0049] The above-mentioned redundant degree of freedom robot joint limitation avoidance method, device, computer equipment, storage medium and computer program product obtain the current joint angle and target posture of the robot; move the limited joint in the robot joint in a preset range of motion in a direction away from the limit to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle; obtain the initial posture of the Cartesian space corresponding to the current joint angle, compare the initial posture and the target posture to obtain the deviation vector, if the deviation vector is not less than the preset error threshold, calculate the pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix; calculate the joint increment based on the deviation vector and the pseudo-inverse Jacobian matrix; update the current joint angle based on the joint increment, and use the updated joint angle as the current joint angle again, return to the step of obtaining the initial posture of the Cartesian space corresponding to the current joint angle, until the latest deviation vector is less than the preset error threshold, and obtain the target joint angle. During the whole process, when there is a limit joint in the robot joint, the limit joint moves in the direction away from the limit within the preset range of motion to break away from the limit, and then based on the constructed dynamic Jacobian matrix, the target joint angle corresponding to the deviation vector being less than the preset error threshold is continuously iterated to solve, which can realize the effective redundant degree of freedom robot to avoid joint limitation. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 A diagram illustrating an application environment of a method for avoiding joint limitation of a redundant degree of freedom robot according to an embodiment;

[0051] Figure 2 1. A schematic flow chart of a method for avoiding joint limitation of a redundant degree of freedom robot according to an embodiment;

[0052] Figure 3 A schematic flow chart of a method for avoiding joint limitation of a redundant degree of freedom robot in another embodiment;

[0053] Figure 4 This is a structural block diagram of a redundant degree of freedom robot joint avoidance limit device in one embodiment;

[0054] Figure 5 FIG. 1 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical solutions and advantages of this application more clear, the following further describes this application in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application.

[0056] In order to explain in detail the technical principle of the method for avoiding joint limitation of the redundant degree of freedom robot in this application, the following will first introduce the relevant contents of the Jacobian matrix.

[0057] Jacobian matrix

[0058] The Jacobian matrix relates the angular velocity of the robot's joint space to the linear velocity and angular velocity in Cartesian space. Let the robot equation be:

[0059] x=x(q)

[0060] It represents the relationship between the position x in Cartesian space and the angle q in joint space. Taking the derivative of both sides of the above equation with respect to time t, we can get the differential relationship between the two.

[0061]

[0062] in Represents the generalized Cartesian velocity, the first three columns are the linear velocity of the end effector, and the last three columns are the angular velocity of the end effector; is the angular velocity of the robot joint; J(q) is a 6×n matrix. Each column element represents the conversion ratio of the joint velocity to the Cartesian velocity when joint i rotates. Therefore, the formula can be written as:

[0063]

[0064] In the above formula It represents the linear velocity and angular velocity of the robot end in Cartesian space when joint i rotates at unit velocity.

[0065] When a robot has redundant degrees of freedom, that is, the number of its joints n is greater than the minimum number of degrees of freedom required to perform the task, the Jacobian matrix is ​​usually a non-square matrix. According to the theory of linear algebra, in this case This equation will have infinite solutions, that is, infinite joint angular velocities Corresponding to the velocity of a Cartesian space

[0066] Dynamic Jacobian Matrix

[0067] According to the previous introduction to the Jacobian matrix, It represents the linear velocity and angular velocity of the robot end in Cartesian space when joint i rotates at unit speed. Obviously, the velocity of the robot end is the sum of the Cartesian velocities caused by the rotation of each joint of the robot. So when joint i reaches the joint limit or approaches the limit, if joint i stops rotating, there will be no contribution from the Cartesian velocity, that is, let That's it. From the perspective of linear algebra, if a column of elements in the matrix is ​​all zero, it will be affected when solving the equation. In particular, if the rank of the matrix is ​​less than the minimum dimension (row or column), the equation may have no solution or an infinite number of solutions. In order to avoid such effects, when joint i approaches or reaches the limit, the elements in the i-th column are removed from the Jacobian matrix, and the original 6×n matrix becomes 6×(n-1). The number of columns removed is related to redundant degrees of freedom. If the degree of freedom of the task is m and the degree of freedom of the robot joint is n, then at most nm columns of elements can be removed. From the perspective of robotics, removing This is equivalent to locking joint i, which means that the joint will not affect the Cartesian velocity. The dynamic Jacobian matrix formed in this way can adapt to the dynamic changes of joint limits during task execution. Let the dynamically generated Jacobian matrix be J d .

[0068] Based on the above principles, the redundant degree of freedom robot joint limitation avoidance method provided in the embodiment of the present application can be applied to Figure 1 In the application environment shown. The controller 102 controls the robot. The controller 102 specifically adopts the redundant degree of freedom robot joint limit avoidance method of the present application to control the robot. The controller 102 obtains the current joint angle and target posture of the robot; moves the limited joint in the robot joint away from the limit within a preset range of motion to obtain a new joint angle, and constructs a dynamic Jacobian matrix based on the adjusted joint angle; obtains the initial posture of the Cartesian space corresponding to the current joint angle, compares the initial posture and the target posture to obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculates the pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix; calculates the joint increment based on the deviation vector and the pseudo-inverse Jacobian matrix; updates the current joint angle based on the joint increment, and uses the updated joint angle as the current joint angle again, and returns to the step of obtaining the initial posture of the Cartesian space corresponding to the current joint angle, until the latest deviation vector is less than the preset error threshold, and obtains the target joint angle to control the robot.

[0069] In one embodiment, Figure 2 As shown in the figure, a method for avoiding joint limitation of redundant degree of freedom robot is provided. Figure 1 The controller 102 in FIG. 1 is taken as an example to illustrate, including the following steps:

[0070] S100: Get the current joint angle and target pose of the robot.

[0071] Start the robot control system and ensure that all sensors and actuators are functioning properly. Read the robot's current joint angles, which reflect the current position of each joint. Based on the task requirements, set the robot's target position and posture in the control system. This is typically done through a human-machine interface (e.g., a touchscreen or keyboard). Furthermore, the target position can also be directly calculated based on the current task.

[0072] S200: moving the limited joints in the robot joints in a direction away from the limit by a preset range of motion to obtain a new joint angle, and constructing a dynamic Jacobian matrix based on the adjusted joint angle.

[0073] Traverse all the joints of the robot and check whether the angle of each joint is close to or has reached its physical limit. This is usually achieved by comparing the current joint angle with the preset limit threshold. For the identified limit joints, adjust their angles in the direction away from the limit by a preset range of motion. The size of this range should be determined according to the specific situation of the robot and the task requirements to ensure that the robot does not reach the limit again in subsequent movements. Based on the adjusted joint angles, use the robot dynamics and kinematics model to construct a dynamic Jacobian matrix. This matrix describes the mapping relationship between joint velocity and end effector velocity and is the basis for subsequent calculations.

[0074] S300: Obtain the initial pose in the Cartesian space corresponding to the current joint angle, compare the initial pose with the target pose, and obtain a deviation vector. If the deviation vector is not less than a preset error threshold, calculate the pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix.

[0075] Using the robot's kinematic model, the robot's initial pose in Cartesian space is calculated based on the current joint angles. The initial pose is compared with the target pose, and the difference between the two is calculated to obtain a deviation vector. This vector reflects the difference between the robot's position and posture and the target pose. If the magnitude of the deviation vector is not less than the preset error threshold, it means that there is still a large gap between the current pose and the target pose, and further adjustment is required. At this point, the pseudo-inverse matrix solution method of the dynamic Jacobian matrix is ​​used to calculate the joint increments that will bring the robot close to the target pose. The solution of the pseudo-inverse matrix can be achieved through numerical methods (such as singular value decomposition and least squares method).

[0076] S400: Calculate the joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix.

[0077] Multiplying the deviation vector by the pseudo-inverse Jacobian matrix yields the joint increments needed to bring the robot closer to the target pose. These increments represent the angle and direction of each joint adjustment. Adding these calculated joint increments to the current joint angles yields the updated joint angles, which serve as the starting point for the next iteration.

[0078] S500: Update the current joint angle based on the joint increment, and use the updated joint angle as the current joint angle again, and return to the step of obtaining the initial pose in the Cartesian space corresponding to the current joint angle until the latest deviation vector is less than the preset error threshold to obtain the target joint angle.

[0079] The updated joint angles are used as the current joint angles, and the above steps are repeated (starting with obtaining the initial Cartesian pose corresponding to the current joint angles) for multiple iterations. Through continuous adjustment and approximation, the size of the deviation vector is gradually reduced until it is less than the preset error threshold. After each iteration, the size of the deviation vector is checked to see if it is less than the preset error threshold. If so, the robot is considered to have reached the target pose and all joints are within the limit state. The iteration process ends, and the corresponding target joint angles are obtained. If not, the next iteration is continued.

[0080] The above-mentioned redundant degree of freedom robot joint limit avoidance method obtains the robot's current joint angle and target pose; moves the limited joint in the robot joint in a preset range of motion in a direction away from the limit to obtain a new joint angle, and constructs a dynamic Jacobian matrix based on the adjusted joint angle; obtains the initial pose in Cartesian space corresponding to the current joint angle, compares the initial pose with the target pose to obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculates the pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix; calculates the joint increment based on the deviation vector and the pseudo-inverse Jacobian matrix; updates the current joint angle based on the joint increment, and uses the updated joint angle as the current joint angle again, returning to the step of obtaining the initial pose in Cartesian space corresponding to the current joint angle, until the latest deviation vector is less than the preset error threshold, and the target joint angle is obtained. Throughout the entire process, when there is a limited joint in the robot joint, the limited joint is moved in a preset range of motion in a direction away from the limit to escape the limit, and then, based on the constructed dynamic Jacobian matrix, it is continuously iteratively solved to obtain the target joint angle corresponding to the deviation vector less than the preset error threshold, thereby effectively achieving redundant degree of freedom robot joint limit avoidance.

[0081] In one embodiment, Figure 3 As shown, S200 includes:

[0082] S220: Move the limited joint in the robot joint in a direction away from the limit within a preset range of motion to obtain a new joint angle.

[0083] First, the robot's current joint angles are read and compared with preset joint limit thresholds to identify which joints are approaching or reaching their limits. For identified joints with limits, their angles are adjusted away from the limits by a preset range of motion. The size of this range should be determined based on the robot's specific situation and task requirements to ensure that these joints do not reach their limits again during subsequent motion. The adjusted joint angles are recorded and used as the basis for subsequent calculations.

[0084] S240: Construct a Jacobian matrix based on the new joint angles, and remove the column elements corresponding to the limit joints in the constructed Jacobian matrix to obtain a dynamic Jacobian matrix.

[0085] Using the robot's dynamic and kinematic models, as well as the adjusted new joint angles, a complete Jacobian matrix is ​​constructed. This matrix describes the mapping between the robot's joint velocities and the end-effector velocities. Since the limit joints have been adjusted and it is desirable to avoid reusing their information in subsequent calculations (because they have approached or reached their limits, which may result in restricted or unstable motion), the column elements corresponding to these limit joints need to be removed from the constructed Jacobian matrix. After this removal operation, the resulting Jacobian matrix is ​​the dynamic Jacobian matrix. This matrix only contains information about joints that are not close to their limits, making it more suitable for subsequent pose adjustments and iterative calculations.

[0086] In one embodiment, comparing the initial posture and the target posture to obtain the deviation vector includes: comparing the initial posture and the target posture, calculating the position deviation and the posture deviation; and combining the position deviation and the posture deviation to obtain the deviation vector.

[0087] In this embodiment, the position deviation and the attitude deviation are calculated by comparing the initial posture and the target posture, and then the position deviation and the attitude deviation are combined to obtain a deviation vector.

[0088] In one embodiment, comparing the initial pose and the target pose, and calculating the position deviation and the attitude deviation includes: subtracting the position portion of the initial pose and the target pose to obtain the position deviation; calculating the inverse matrix product of the attitude portion of the initial pose and the target pose; and converting the inverse matrix product into an axis-angle form to obtain the attitude deviation.

[0089] In a specific application example, the initial posture T init and target pose T target Calculate the deviation Δx between the two. The first three columns of Δx are position deviations, which only require the initial pose T initand target pose T target The last three columns of Δx are the posture deviations. Let the initial posture T init and target pose T target The posture parts are R init and R target ,but Then converting ΔR into axis-angle form gives the last three columns of Δx.

[0090] In one embodiment, obtaining the initial Cartesian space pose corresponding to the current joint angle includes: substituting the current joint angle into the robot's forward kinematics to calculate the robot's initial Cartesian space pose.

[0091] In the field of robot control, it is often necessary to know the Cartesian space pose (position and posture) of the robot at a specific joint angle. This is crucial for achieving tasks such as accurate path planning, obstacle avoidance, and object grasping. This embodiment aims to elaborate on how to obtain the initial pose of the robot in Cartesian space from the current joint angle through robot forward kinematics calculation. Robot forward kinematics refers to the mapping relationship from joint space (i.e., joint angle) to task space (i.e., Cartesian space). For a serial robot (such as a six-axis industrial robot), its forward kinematic model can usually be expressed as:

[0092] T=A1·A2·…·An.

[0093] Where T is the pose matrix of the end effector, and Ai is the transformation matrix of the i-th joint, which describes the change in pose of that joint relative to the previous joint. These transformation matrices typically consist of rotational and translational components, and their specific form depends on the type of joint (e.g., revolute joint, translational joint, etc.). In a robot control system, the actual angles of the current joint are typically obtained through sensors (e.g., encoders). This angle data serves as input for the robot's forward kinematics calculations. Read the current joint angles. Obtain the current angle values ​​of all joints through the control system interface. Construct the transformation matrix. Based on the robot's structural parameters (e.g., link length, joint offset, etc.) and the current joint angles, construct the transformation matrix Ai for each joint. Calculate the end effector pose matrix. Multiply the transformation matrices of all joints sequentially to obtain the end effector pose matrix T. Extract the position and pose. Extract the end effector's position vector (usually located in the first three rows and last column) and pose matrix (usually located in the first three rows and first three columns) from the pose matrix T.

[0094] In one embodiment, the predetermined range of motion includes 5% of the joint's total range of motion.

[0095] The preset range of motion can be set based on the actual application scenario. It is specifically determined by the robot's speed parameters. Just make sure the robot's speed can keep up with adjustments to avoid limiting. Generally speaking, the preset range of motion can be 5% of the joint's total range of motion. When a joint is detected approaching or reaching a limit, the joint is moved an additional 5% of its total range of motion away from the limit.

[0096] In one specific application example, the method for avoiding joint limitation of a redundant degree of freedom robot of the present application includes the following steps:

[0097] (1) Input the robot's current joint state q init and the target pose T in Cartesian space target .

[0098] (2) Determine whether there is a joint approaching the limit. If there is q i Close to the limit, let q i Move 5% of the joint range away from the joint limit to obtain q i_target Then, according to the construction method of the dynamic Jacobian matrix, remove the elements in the i-th column and obtain the Jacobian matrix J d ; If no joint is close to the limit, then J d =J.

[0099] (3) Put q init Substitute the robot's forward kinematics to calculate the robot's initial Cartesian space pose T init

[0100] (4) According to T init and T target Calculate the deviation Δx between the two. The first three columns of Δx are position deviations, which only need to be calculated using T init and T target The last three columns of Δx are the attitude deviations, let T init and T target The posture parts are R init and R target ,but Then, converting ΔR into the axis-angle form gives the last three columns of Δx. If each value of Δx is less than the specified error ε, execute (8), otherwise continue from (5).

[0101] (5) Calculate the pseudo-inverse of the Jacobian matrix:

[0102] (6) Calculate joint increment: Δq only includes the joint increments that have not reached the joint limits.

[0103] (7) Update q init Value: q init =q init +Δq. It should be noted that if the original q init Some joints are close to the limit, so update q init When i_target . Then continue with (3).

[0104] (8) Get the final target angle q target , the algorithm ends.

[0105] In practical applications, without the method of the present invention, if a redundant robot encounters a limit while tracking a task path, the inverse solution calculation will fail, and the robot will also report a joint out-of-limit error. With the method of the present invention, even if a joint axis encounters a limit, the redundant robot's redundancy can be leveraged to allow the joint near the limit to move a short distance away from the limit, then escape the limit, ensuring that the robot can track and complete the task path.

[0106] It should be understood that, although the steps in the flowcharts of the above embodiments are shown in sequence as indicated by the arrows, these steps are not necessarily performed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and these steps can be performed in other orders. Moreover, at least a portion of the steps in the flowcharts of the above embodiments may include multiple steps or multiple stages, and these steps or stages are not necessarily performed at the same time, but can be performed at different times. The execution order of these steps or stages is not necessarily to be performed in sequence, but can be performed in turn or alternately with other steps or at least a portion of steps or stages in other steps.

[0107] Based on the same inventive concept, the present application also provides a redundant degree of freedom robot joint avoidance limiting device for implementing the redundant degree of freedom robot joint avoidance limiting method mentioned above. The solution provided by this device is similar to the solution described in the above method. Therefore, the specific limitations of one or more redundant degree of freedom robot joint avoidance limiting device embodiments provided below can be found in the limitations of the redundant degree of freedom robot joint avoidance limiting method described above, and will not be repeated here.

[0108] In one embodiment, Figure 4 As shown, a redundant degree of freedom robot joint limit avoidance device is provided, comprising:

[0109] The parameter acquisition module 100 is used to obtain the current joint angle and target posture of the robot;

[0110] The fine-tuning module 200 is used to move the limited joints in the robot joints in a direction away from the limit by a preset range of motion to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle;

[0111] Deviation calculation module 300, used to obtain the initial pose in Cartesian space corresponding to the current joint angle, compare the initial pose with the target pose, obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculate the pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix;

[0112] An increment calculation module 400 is used to calculate the joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix;

[0113] The iterative adjustment module 500 is used to update the current joint angle based on the joint increment, and use the updated joint angle as the current joint angle, and control the deviation calculation module to re-execute the operation of obtaining the initial posture in the Cartesian space corresponding to the current joint angle until the latest deviation vector is less than the preset error threshold, thereby obtaining the target joint angle.

[0114] In one embodiment, the fine-tuning module 200 is also used to move the limited joints in the robot joints in a preset range of motion in a direction away from the limit to obtain a new joint angle; construct a Jacobian matrix based on the new joint angle, and eliminate the column elements corresponding to the limited joints in the constructed Jacobian matrix to obtain a dynamic Jacobian matrix.

[0115] In one embodiment, the deviation calculation module 300 is further configured to compare the initial posture and the target posture, calculate the position deviation and the posture deviation, and combine the position deviation and the posture deviation to obtain a deviation vector.

[0116] In one embodiment, the deviation calculation module 300 is further used to subtract the position part of the initial posture and the target posture to obtain the position deviation; calculate the inverse matrix product of the posture part of the initial posture and the target posture; and convert the inverse matrix product into axis-angle form to obtain the posture deviation.

[0117] In one embodiment, the deviation calculation module 300 is further configured to substitute the current joint angle into the robot's forward kinematics to calculate the robot's initial Cartesian space pose.

[0118] In one embodiment, the predetermined range of motion includes 5% of the joint's total range of motion.

[0119] Each module in the redundant-degree-of-freedom robot joint limit avoidance device described above can be implemented in whole or in part through software, hardware, or a combination thereof. Each module can be embedded in or independent of a processor in a computer device in hardware form, or can be stored in a computer device memory in software form, so that the processor can call and execute the corresponding operations of each module.

[0120] In one embodiment, a computer device is provided. The computer device may be a terminal, and its internal structure diagram may be as follows: Figure 5 As shown. The computer device includes a processor, a memory, a communication interface, a display screen and an input device connected via a system bus. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the computer device is used to communicate with an external terminal in a wired or wireless manner, and the wireless manner can be achieved through WIFI, a mobile cellular network, NFC (near field communication) or other technologies. When the computer program is executed by the processor, a method for avoiding joint limitation of a redundant degree of freedom robot is implemented. The display screen of the computer device can be a liquid crystal display screen or an electronic ink display screen, and the input device of the computer device can be a touch layer covering the display screen, or a button, trackball or touchpad provided on the computer device housing, or an external keyboard, touchpad or mouse.

[0121] Those skilled in the art will understand that Figure 5 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement.

[0122] In one embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and when the processor executes the computer program, the above-mentioned redundant degree of freedom robot joint limitation avoidance method is implemented.

[0123] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the above-mentioned redundant degree of freedom robot joint limitation avoidance method is implemented.

[0124] In one embodiment, a computer program product is provided, comprising a computer program, which, when executed by a processor, implements the above-mentioned redundant degree of freedom robot joint limitation avoidance method.

[0125] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, database or other media used in the embodiments provided in this application may include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory may include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in the various embodiments provided herein may include at least one of a relational database and a non-relational database. Non-relational databases may include, but are not limited to, distributed databases based on blockchains. The processor involved in the various embodiments provided herein may be, but are not limited to, a general-purpose processor, a central processing unit, a graphics processing unit, a digital signal processor, a programmable logic unit, a data processing logic unit based on quantum computing, and the like.

[0126] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0127] The above embodiments merely illustrate several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art may make various modifications and improvements without departing from the spirit of the present invention, all of which fall within the scope of protection of the present application. Therefore, the scope of protection of the present application shall be determined by the appended claims.

Claims

1. A method for avoiding joint limitation of a redundant degree of freedom robot, characterized in that: The method comprises: Get the robot's current joint angle and target pose; Move the limited joints in the robot joints in a preset range of motion in a direction away from the limit to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle; Obtaining an initial pose in Cartesian space corresponding to the current joint angle, comparing the initial pose with the target pose to obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculating a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix; Calculating a joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix; The current joint angle is updated based on the joint increment, and the updated joint angle is used as the current joint angle again, and the step of obtaining the initial posture in the Cartesian space corresponding to the current joint angle is returned until the latest deviation vector is less than the preset error threshold to obtain the target joint angle.

2. The method according to claim 1, characterized in that The method of moving the limited joint of the robot joint in a direction away from the limit to a preset range of motion to obtain a new joint angle, and constructing a dynamic Jacobian matrix based on the adjusted joint angle includes: Move the limited joints in the robot joints away from the limit position in the preset range of motion to obtain a new joint angle; A Jacobian matrix is ​​constructed based on the new joint angles, and the column elements corresponding to the limit joints in the constructed Jacobian matrix are eliminated to obtain a dynamic Jacobian matrix.

3. The method according to claim 1, characterized in that Comparing the initial pose and the target pose to obtain a deviation vector includes: Comparing the initial position and the target position to calculate the position deviation and the attitude deviation; The position deviation and attitude deviation are combined to obtain the deviation vector.

4. The method according to claim 3, characterized in that Comparing the initial posture and the target posture and calculating the position deviation and the posture deviation includes: Subtracting the position portion of the initial posture from the target posture to obtain a position deviation; Calculating the inverse matrix product of the initial pose and the pose portion of the target pose; The inverse matrix product is converted into an axis-angle form to obtain the attitude deviation.

5. The method according to claim 1, wherein Obtaining the initial Cartesian space pose corresponding to the current joint angle includes: Substitute the current joint angle into the robot's forward kinematics to calculate the robot's initial Cartesian space pose.

6. The method according to claim 1, wherein The preset range of motion includes 5% of the overall range of motion of the joint.

7. A redundant degree of freedom robot joint limit avoidance device, characterized in that: The device comprises: Parameter acquisition module, used to obtain the robot's current joint angle and target posture; A fine-tuning module is used to move the limited joints in the robot's joints in a preset range of motion in a direction away from the limit to obtain a new joint angle, and construct a dynamic Jacobian matrix based on the adjusted joint angle; a deviation calculation module, configured to obtain an initial Cartesian space pose corresponding to the current joint angle, compare the initial pose with the target pose, obtain a deviation vector, and calculate a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix if the deviation vector is not less than a preset error threshold; an increment calculation module, configured to calculate a joint increment based on the deviation vector and the pseudo-inverse Jacobian matrix; An iterative adjustment module is used to update the current joint angle based on the joint increment, and to use the updated joint angle as the current joint angle, and to control the deviation calculation module to re-execute the operation of obtaining the initial posture in the Cartesian space corresponding to the current joint angle until the latest deviation vector is less than the preset error threshold, thereby obtaining the target joint angle.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

10. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images