Redundant-degree-of-freedom robot joint avoiding limiting method, device and equipment

Abstract

The invention relates to a joint avoiding limiting method, device and equipment for a redundant degree-of-freedom robot, and the method comprises the steps: obtaining a current joint angle and a target pose of the robot; the limited joint in the robot joints moves in the direction away from the limited position by a preset movement range, and a dynamic Jacobian matrix is constructed based on the adjusted joint angle; obtaining an initial pose of a Cartesian space corresponding to the current joint angle, comparing the initial pose with a target pose to obtain a deviation vector, and if the deviation vector is not smaller than a preset error threshold value, calculating a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix; calculating a joint increment according to the deviation vector and a pseudo-inverse Jacobian matrix; and updating the current joint angle based on the joint increment, taking the updated joint angle as the current joint angle again, and returning to the step of obtaining the initial pose of the Cartesian space corresponding to the current joint angle until the newly obtained deviation vector is smaller than a preset error threshold value, thereby obtaining a 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 degree of freedom robot. Background Art

[0002] With the rapid development of automation and robotics, redundant robotic arms have shown great 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 operating capabilities.

[0003] In application scenarios such as welding and gluing, robots are usually required to accurately track along a specific task path. Such tasks require that the robot cannot be interrupted during execution and must complete the tracking of the entire task path at one time. However, in actual applications, especially when the task path is long or the path complexity is high, even robots with redundant degrees of freedom may face the problem of joint limitations. Joint limitation means that one or some joints of the robot can no longer continue normal movement after reaching their mechanical or physical limits, which often leads to the failure of task execution.

[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] The limited joints in the robot joints are moved in a preset range of motion in a direction away from the limit to obtain a new joint angle, and a dynamic Jacobian matrix is ​​constructed based on the adjusted joint angle;

[0009] Obtaining an initial posture in the Cartesian space corresponding to the current joint angle, comparing the initial posture with the target posture 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 the limited joint in the 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 in a direction away from the limit within a preset range of motion to obtain a new joint angle;

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

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

[0016] Comparing the initial position and the target position to calculate the position deviation and the attitude deviation;

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

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

[0019] Subtracting the position part 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 preset range of motion includes 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 current joint angle and target posture of the robot;

[0027] A fine-tuning module is 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, and construct a dynamic Jacobian matrix based on the adjusted joint angle;

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

[0029] An increment calculation module, used for calculating a joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix;

[0030] The iterative adjustment module is used to update the current joint angle based on the joint increment, and use the updated joint angle as the current joint angle again, 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.

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

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

[0033] The limited joints in the robot joints are moved in a preset range of motion in a direction away from the limit to obtain a new joint angle, and a dynamic Jacobian matrix is ​​constructed based on the adjusted joint angle;

[0034] Obtaining an initial posture in the Cartesian space corresponding to the current joint angle, comparing the initial posture with the target posture 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. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the following steps are implemented:

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

[0039] The limited joints in the robot joints are moved in a preset range of motion in a direction away from the limit to obtain a new joint angle, and a dynamic Jacobian matrix is ​​constructed based on the adjusted joint angle;

[0040] Obtaining an initial posture in the Cartesian space corresponding to the current joint angle, comparing the initial posture with the target posture 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, and when the computer program is executed by a processor, the following steps are implemented:

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

[0045] The limited joints in the robot joints are moved in a preset range of motion in a direction away from the limit to obtain a new joint angle, and a dynamic Jacobian matrix is ​​constructed based on the adjusted joint angle;

[0046] Obtaining an initial posture in the Cartesian space corresponding to the current joint angle, comparing the initial posture with the target posture 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 with the target posture, and 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, and 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 the target joint angle is obtained. 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 escape 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 solved iteratively to realize effective redundant degree of freedom robot avoiding joint limitation. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 A diagram showing an application environment of a redundant degree of freedom robot joint limitation avoidance method in one embodiment;

[0051] Figure 2 It is a schematic flow chart of a method for avoiding joint limitation of a redundant degree of freedom robot in one 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 It is a structural block diagram of a redundant degree of freedom robot joint limit avoidance device in one embodiment;

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

[0055] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0056] In order to explain in detail the technical principle of the redundant freedom robot joint limitation avoidance method of the present application, the following will first introduce the contents related to 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 Cartesian space position x and the joint space angle q. 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 It 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 the 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 the 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, 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 speed of the robot end is the superposition of the Cartesian speed caused by the rotation of each joint of the robot. Therefore, when joint i reaches the joint limit or approaches the limit, stopping the rotation of joint i will not produce the contribution of Cartesian speed, that is, 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 (rows or columns), the equation may have no solution or may have infinite 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 that are removed is related to redundant degrees of freedom. Suppose 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 principle, the redundant degree of freedom robot joint limit 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 uses 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 by 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 with the target posture, and obtains the deviation vector. If the deviation vector is not less than the preset error threshold, the pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix is ​​calculated; the joint increment is calculated 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 of the Cartesian space corresponding to the current joint angle is returned until the latest deviation vector is less than the preset error threshold, and the target joint angle is obtained to control the robot.

[0069] In one embodiment, Figure 2 As shown, a redundant degree of freedom robot joint limit avoidance method is provided, and the method is applied to Figure 1 The controller 102 in the example is used as an example to illustrate, and the following steps are included:

[0070] S100: Obtain the current joint angle and target posture of the robot.

[0071] Start the robot control system and ensure that all sensors and actuators are working properly. Read the current joint angles of the robot, which reflect the current positions of the robot's joints. According to the task requirements, set the robot's target position and posture in the control system, including the target position and posture, which is usually done through a human-computer interface (such as a touch screen, keyboard, etc.). 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 and task requirements of the robot to ensure that the robot does not reach the limit again in subsequent movements. Based on the adjusted joint angles, a dynamic Jacobian matrix is ​​constructed using the robot dynamics and kinematics model. This matrix describes the mapping relationship between the joint velocity and the end effector velocity, and is the basis for subsequent calculations.

[0074] S300: Obtain the initial posture of the Cartesian space corresponding to the current joint angle, compare the initial posture with the target posture, obtain the 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.

[0075] Using the robot's kinematic model, the robot's initial posture in Cartesian space is calculated based on the current joint angles. The initial posture is compared with the target posture, 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 posture. If the size of the deviation vector is not less than the preset error threshold, it means that there is still a large gap between the current posture and the target posture, and further adjustment is required. At this time, the pseudo-inverse matrix solution method of the dynamic Jacobian matrix is ​​used to calculate the joint increment that can make the robot close to the target posture. The solution of the pseudo-inverse matrix can be achieved through numerical methods (such as singular value decomposition, least squares method, etc.).

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

[0077] Multiply the deviation vector with the pseudo-inverse Jacobian matrix to obtain the joint increment that can make the robot close to the target posture. This increment represents the angle and direction that each joint needs to adjust. Add the calculated joint increment to the current joint angle to obtain the updated joint angle, which will be used 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 posture in the 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.

[0079] The updated joint angle is used as the current joint angle again, and the above steps are repeated (starting from obtaining the initial position of the Cartesian space corresponding to the current joint angle) 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, check whether the size of the deviation vector is less than the preset error threshold. If so, it is considered that the robot has reached the target position and all joints have not reached the limit state. The iteration process ends and the corresponding target joint angle is obtained. If not, proceed to the next iteration.

[0080] The above redundant degree of freedom robot joint limit avoidance method obtains the current joint angle and target posture of the robot; moves the limited joint in the robot joint to the preset range of activity in the 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 posture of the Cartesian space corresponding to the current joint angle, compares the initial posture and the target posture, obtains the deviation vector, and if the deviation vector is not less than the 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. In the whole process, when there is a limited joint in the robot joint, the limited joint moves the preset range of activity in the direction away from the limit, escapes from the limit, and then continuously iterates based on the constructed dynamic Jacobian matrix to solve the target joint angle corresponding to the deviation vector less than the preset error threshold, which can realize the effective redundant degree of freedom robot joint limit avoidance.

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

[0082] S220: 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.

[0083] First, the robot's current joint angles are read and compared with the preset joint limit thresholds to identify which joints are close to or have reached their limits. For the identified limit joints, their angles are adjusted away from the limit 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 in subsequent movements. The adjusted joint angles are recorded 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 dynamics and kinematics model, as well as the adjusted new joint angles, a complete Jacobian matrix is ​​constructed. This matrix describes the mapping relationship between the robot joint velocity and the end effector velocity. Since the limit joints have been adjusted and it is hoped to avoid reusing the information of these joints in subsequent calculations (because they have approached or reached the limit, which may result in restricted or unstable motion), it is necessary to remove the column elements corresponding to these limit joints from the constructed Jacobian matrix. After the removal operation, the resulting Jacobian matrix is ​​the dynamic Jacobian matrix. This matrix only contains information about joints that are not close to the limit, so it is more suitable for subsequent posture adjustment 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; 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 posture and the target posture, calculating the position deviation and the attitude deviation includes: subtracting the position part of the initial posture and the target posture to obtain the position deviation; calculating the inverse matrix product of the attitude part of the initial posture and the target posture; 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 convert ΔR into axis-angle form to get the last three columns of Δx.

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

[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 essential for achieving accurate path planning, obstacle avoidance, object grasping and other tasks. 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 kinematics model can usually be expressed as:

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

[0093] Among them, T is the pose matrix of the end effector, Ai is the transformation matrix of the i-th joint, which describes the pose change of the joint relative to the previous joint. These transformation matrices usually include rotation and translation, and the specific form depends on the type of joint (such as rotation joint, translation joint, etc.). In the robot control system, the actual angle of the current joint is usually obtained through sensors (such as encoders). These angle data are the input of the robot's forward kinematics calculation. Read the current joint angle. Obtain the current angle values ​​of all joints through the control system interface. Construct the transformation matrix. Construct the transformation matrix Ai of each joint based on the robot structural parameters (such as link length, joint offset, etc.) and the current joint angle. Calculate the pose matrix of the end effector. Multiply the transformation matrices of all joints in sequence to obtain the pose matrix T of the end effector to extract the position and posture. Extract the position vector (usually located in the last column of the first three rows of the matrix) and the posture matrix (usually located in the first three rows and first three columns of the matrix) of the end effector from the pose matrix T.

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

[0095] The preset range of motion can be set according to the actual application scenario. It is specifically based on the speed parameters of the robot. It only needs to ensure that the speed of the robot can keep up with the adjustment to avoid limit. Generally speaking, the preset range of motion can be 5% of the overall range of motion of the joint. When a joint is detected to be close to or has been limited, these joints are moved away from the joint limit by 5% of the entire range of motion of the joint.

[0096] In one specific application example, the redundant freedom robot joint limit avoidance method 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 any joint is currently close to 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 position 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 the 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 contains 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 to execute (3).

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

[0105] In practical applications, when the method of the present invention is not used, when the redundant robot is tracking the task path, encountering a limit will cause the inverse solution calculation to fail, and the robot will also report an error of joint over-limit. After using the method of the present invention, even if the joint axis encounters a limit, the redundant characteristics of the redundant robot can be used to allow the joint close to the limit to move a short distance in the direction away from the limit, and then break away from 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 involved in the above embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps is not strictly limited in order, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above embodiments may include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0107] Based on the same inventive concept, the embodiment of 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 involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in the embodiments of one or more redundant degree of freedom robot joint avoidance limiting devices provided below can refer to the limitations of the redundant degree of freedom robot joint avoidance limiting method 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] A 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] The deviation calculation module 300 is used to obtain the initial posture of the Cartesian space corresponding to the current joint angle, compare the initial posture with the target posture, obtain the 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 a joint increment according to a deviation vector and a 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 of the embodiments, the deviation calculation module 300 is further used 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 also 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 used to substitute the current joint angle into the robot forward kinematics to calculate the initial position and posture of the robot in Cartesian space.

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

[0119] Each module in the redundant degree of freedom robot joint limit avoidance device can be implemented in whole or in part by software, hardware and a combination thereof. Each module can be embedded in or independent of a processor in a computer device in the form of hardware, or can be stored in a memory in a computer device in the form of software, so that the processor can call and execute operations corresponding to 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 through a system bus. Among them, 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 redundant degree of freedom robot joint limit avoidance method 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 covered on the display screen, or a button, trackball or touchpad set on the computer device housing, or an external keyboard, touchpad or mouse, etc.

[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 those shown in the figure, or combine certain components, or have a different arrangement of components.

[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, including 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 can understand that all or part of the processes in the above-mentioned embodiments can be completed 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 the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an 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 each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.

[0126] The technical features of the above embodiments may 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 only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached 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 posture; The limited joints in the robot joints are moved in a preset range of motion in a direction away from the limit to obtain a new joint angle, and a dynamic Jacobian matrix is ​​constructed based on the adjusted joint angle; Obtaining an initial posture in the Cartesian space corresponding to the current joint angle, comparing the initial posture with the target posture 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 joints in the robot joints in a direction away from the limited joints within a preset range of motion to obtain new joint angles, and constructing a dynamic Jacobian matrix based on the adjusted joint angles includes: Move the limited joints in the robot joints in a direction away from the limit within a preset range of motion to obtain a new joint angle; A Jacobian matrix is ​​constructed based on the new joint angles, and column elements corresponding to the limit joints in the constructed Jacobian matrix are removed to obtain a dynamic Jacobian matrix.

3. The method according to claim 1, characterized in that Comparing the initial pose and the target pose, the deviation vector obtained 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 a deviation vector.

4. The method according to claim 3, characterized in that The comparing the initial posture and the target posture and calculating the position deviation and the posture deviation comprises: Subtracting the position part 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, characterized in that Obtaining the initial pose of the Cartesian space 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, characterized in that The preset range of motion includes 5% of the overall range of motion of the joint.

7. A redundant freedom robot joint limit avoidance device, characterized in that: The device comprises: Parameter acquisition module, used to obtain the current joint angle and target posture of the robot; A fine-tuning module is 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, and construct a dynamic Jacobian matrix based on the adjusted joint angle; a deviation calculation module, used to obtain an initial posture in the Cartesian space corresponding to the current joint angle, compare the initial posture with the target posture, obtain a deviation vector, and if the deviation vector is not less than a preset error threshold, calculate a pseudo-inverse Jacobian matrix of the dynamic Jacobian matrix; An increment calculation module, used for calculating a joint increment according to the deviation vector and the pseudo-inverse Jacobian matrix; The iterative adjustment module is used to update the current joint angle based on the joint increment, and use the updated joint angle as the current joint angle again, 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.

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