Joint limiting avoiding method for redundant mechanical arm
By calculating the joint limit avoidance increment and speed adaptive adjustment of the redundant robotic arm, the problem of unpredictable elbow movement and poor joint limit avoidance during the redundant robotic arm movement is solved, and the robotic arm is able to achieve different speeds stable movement under.
Patent Information
- Application Number
- CN202411065636.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-05
- Publication Date
- 2024-11-15
AI Technical Summary
The prior art has the problem of joint speeding due to the different movements of the redundant robotic arm during the movement of the redundant robotic arm.
By obtaining the current joint angle and posture data of the redundant robotic arm, calculate the posture increment and Jacobian matrix, use variable weight function and zero-space projection matrix to calculate the joint limit avoidance increment, and judge the angle output from the joint running speed. and adjust the speed adaptively to avoid speeding.
It realizes the certainty of the movement of the robotic arm elbow and effective limit avoidance, solves the joint speeding problem caused by different amplitudes of avoiding movements at different speeds, and improves the real-time and computing efficiency of the algorithm.
Smart Images

Figure CN118952194A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of robot arm control, and in particular to a joint limit avoidance method of a redundant robot arm. Background Art
[0002] At present, the most commonly used joint limit avoidance method is the equal weight method mentioned in the literature such as An Integrated Inverse Kinematic Approach for the 7-DOF Humanoid Arm with Offset Wrist, Inverse Kinematics and Control of a 7-DOF Redundant Manipulator Based on the Closed-Loop Algorithm (An Inverse Kinematic Approach for the 7-DOF Humanoid Arm with Offset Wrist, Inverse Kinematics and Control of a 7-DOF Redundant Manipulator Based on the Closed-Loop Algorithm). However, this method cannot achieve effective boundary avoidance in some cases. Taking the SRS (Spherical-Roll-Spherical, meaning that the three joints at the base end of the robot and the three joints at the end intersect at one point) configuration 7-DOF manipulator as an example, when the angle of joint 5 is close to 0, the limit avoidance of joint 6 will fail; in order to solve this problem, Redundancy Resolution With Multiple Criteria, Clamping Weighted Least-Norm Method for the Manipulator Kinematic Control: Avoiding Joint Limits (a multi-standard redundant solution to avoid joint limitations, a clamping weighted minimum norm method for manipulator motion control) and others proposed a variable weight method. However, this method makes the elbow movement of the robot arm difficult to predict during movement, and the latter is very complex to calculate, which is not conducive to the real-time deployment of the algorithm.
[0003] In addition, the methods proposed in the literature such as Real-time flow optimization of hydraulic manipulator with one degree of redundancy considering joint limit constraint, A Weighted Least-Norm Solution Based Scheme for Avoiding Joint Limits for Redundant JointManipulators (Real-time flow optimization of single-level redundant hydraulic manipulator considering joint limit constraint, redundant joint manipulator avoiding joint limit scheme based on weighted minimum normative solution) will cause the joint to have an avoidance speed tending to infinity when approaching the limit, which will cause the robot arm to vibrate or hardware damage due to excessive acceleration; the literature Robot Control near Singularity and Joint Limit Using a Continuous Task The method proposed by TransitionAlgorithm (using continuous task transition algorithm to realize robot control near singularity and joint limit) only works within the set threshold, that is, the avoidance action is activated when the distance from the joint angle to the joint limit is less than the threshold. The problem with this method is that the basic configuration of the elbow joint of the redundant robot arm remains unchanged when the avoidance action is not activated, but the configuration depends on the operation of the previous trajectory, which will cause the robot arm elbow configuration to be different when the previous motion of the same motion trajectory is different, that is, the movement of the elbow cannot be predicted. If the elbow movement cannot be effectively predicted, the elbow may collide with the environment.
[0004] In addition to the above-mentioned technical problems, the prior art also has the problem that when deployed in position control mode, the amplitude of the avoidance action is different at different operating speeds. When the robot runs at a slower speed, the joint overspeed problem may occur due to excessive avoidance actions. Summary of the invention
[0005] (I) Purpose of the invention
[0006] The purpose of the present invention is to provide a joint limit avoidance method for a redundant robotic arm to solve the problems of unpredictable elbow movement of the robotic arm during movement, inability to achieve effective avoidance by joint limits, and joint overspeed caused by different avoidance action amplitudes of the robot at different operating speeds.
[0007] (II) Technical solution
[0008] In order to solve the above problems, the present invention provides a joint limit avoidance method for a redundant mechanical arm, comprising:
[0009] S1: Obtain the current joint angle, current posture data and posture data to be solved of the redundant robot arm;
[0010] S2: determining a posture increment according to the current joint angle, the current posture data and the posture data to be solved;
[0011] S3: determining redundant parameters based on the posture increment and the current joint angle, solving the redundant parameters and selecting a set of inverse solutions as the current inverse solution result;
[0012] S4: Calculate the corresponding Jacobian matrix according to the current joint angle, and solve the null space projection matrix corresponding to the current robot arm configuration through the Jacobian matrix;
[0013] S5: Calculate the variable weight function of each joint position, and calculate the joint limit avoidance increment of each joint position using the variable weight function and the null space projection matrix;
[0014] S6: Calculating the joint running speed according to the current joint angle and the current inverse solution result;
[0015] S7: Determine a joint limit avoidance angle output value based on the current inverse solution result, the joint limit avoidance increment and the joint running speed;
[0016] S8: Determine whether the joint limit avoidance angle output value is overspeed, and output the actual output angle of the joint according to the judgment result.
[0017] Furthermore, step S8 specifically includes:
[0018] Determine whether the joint limit avoidance angle output value exceeds the speed limit,
[0019] If yes, then the joint angle corresponding to the current inverse solution result is output as the actual output angle of the joint;
[0020] If not, the joint limit avoidance angle output value is used as the actual joint output angle.
[0021] Furthermore, step S7 specifically includes:
[0022] Calculate the distance d from the current position of the redundant robot arm to the joint limit center point;
[0023] Estimating the number of control cycles n of running to the joint limit center point at the joint running speed;
[0024] The joint limit avoidance increment is reduced by n times, and is applied to the current inverse solution result for output to obtain a joint angle output value.
[0025] Furthermore, the calculation formula of the joint running speed is as follows:
[0026]
[0027] in, is the joint running speed, q solve is the current inverse solution result, q ref is the current joint angle, and dt is the control period.
[0028] Furthermore, the calculation formula for the distance d from the current position of the redundant mechanical arm to the joint limit center point is as follows:
[0029]
[0030] In the formula, q max Indicates the maximum limit of the joint, q min Indicates the minimum limit of the joint.
[0031] Furthermore, the calculation formula of the control cycle number n is as follows:
[0032]
[0033] In the formula, max(·) is the vector maximum operator (i.e., the maximum value of the vector), and round(·) is the rounding operator (i.e., taking the integer value);
[0034] Furthermore, the calculation formula of the joint angle output value is as follows:
[0035]
[0036] Where Δq is the joint limit avoidance increment, Output value of joint limit avoidance angle.
[0037] Furthermore, solving the redundant parameters and selecting a set of inverse solutions as the current inverse solution result includes: using spatial geometric relationships to solve multiple sets of inverse solutions for redundant robotic arms, and selecting a set of inverse solutions that is closest to the current joint angle from the multiple sets of inverse solutions to obtain the current inverse solution result.
[0038] Furthermore, the calculation formula of the joint limit avoidance increment is as follows:
[0039]
[0040] Where k is the avoidance velocity coefficient, C(q) is the variable weight function matrix, which is in the form of a diagonal matrix, consisting of the variable weight function c of each joint i (q i )composition, is the null space projection matrix, is the gradient of the task function H(q) for joint limit avoidance.
[0041] Furthermore, the variable weight function c of each joint i (q i ) is calculated as follows:
[0042] c i (q i )=Acos(π-πt i )+A+1;
[0043] Where A is the maximum escape ratio, t i is the damping coefficient.
[0044] (III) Beneficial effects
[0045] The above-mentioned technical scheme of the present invention has the following beneficial technical effects: the present invention designs a new variable weight method different from the existing technical scheme, and uses the variable weight function and the null space projection matrix to calculate the joint limit avoidance increment of each joint position, thereby solving the problem that the elbow movement of the robot arm cannot be predicted during movement and the joint limit cannot be effectively avoided. In order to solve the problem of different limit avoidance effects at different speeds, the present invention also designs a speed-adaptive avoidance method, the principle of which is: based on the current inverse solution result, the joint limit avoidance increment and the joint running speed, determine the joint limit avoidance angle output value; finally, by judging whether the joint limit avoidance angle output value exceeds the speed, the actual output angle of the joint is output, thereby solving the problem of joint overspeed caused by the different avoidance action amplitudes of the robot at different running speeds. BRIEF DESCRIPTION OF THE DRAWINGS
[0046] Figure 1 This is a flow chart of a joint limit avoidance method based on the redundant mechanical arm of the present invention;
[0047] Figure 2 is a first joint damping coefficient curve of a specific embodiment of the present invention;
[0048] Figure 3 is a first joint variable weight coefficient curve of a specific embodiment of the present invention;
[0049] Figure 4 1 is a comparison diagram of the position limiting avoidance of the sixth joint of a specific embodiment of the present invention and a conventional solution;
[0050] Figure 5 is a comparison diagram of the first joint of a specific embodiment of the present invention and the limit avoidance of a conventional solution;
[0051] Figure 6 1 is a comparison diagram of the position limiting avoidance of the second joint of a specific embodiment of the present invention and a conventional solution;
[0052] Figure 7 is a comparison diagram of the third joint of a specific embodiment of the present invention and the limit avoidance of a conventional solution;
[0053] Figure 8 is a comparison diagram of the fourth joint of a specific embodiment of the present invention and the limit avoidance of a conventional solution;
[0054] Fig. 9 1. is a comparison diagram of the fifth joint of a specific embodiment of the present invention and the conventional solution in terms of position limiting avoidance;
[0055] Fig.10 It is a comparison diagram of the limit avoidance of the seventh joint of the specific embodiment of the present invention and the conventional solution. DETAILED DESCRIPTION
[0056] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings. It should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present invention. In addition, in the following description, the description of well-known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present invention.
[0057] The present invention provides a method for avoiding joint limit of a redundant mechanical arm, such as Figure 1 As shown, including:
[0058] S1: Obtain the current joint angle, current posture data and posture data to be solved of the redundant robot arm;
[0059] S2: Determine the posture increment according to the current joint angle, the current posture data and the posture data to be solved;
[0060] S3: Determine redundant parameters based on the posture increment and the current joint angle, solve the redundant parameters and select a set of inverse solutions as the current inverse solution result;
[0061] S4: Calculate the corresponding Jacobian matrix according to the current joint angle, and solve the null space projection matrix corresponding to the current robot arm configuration through the Jacobian matrix;
[0062] S5: Calculate the variable weight function of each joint position, and calculate the joint limit avoidance increment of each joint position using the variable weight function and the null space projection matrix;
[0063] S6: Calculate the joint running speed according to the current joint angle and the current inverse solution result;
[0064] S7: Determine the joint limit avoidance angle output value based on the current inverse solution result, the joint limit avoidance increment and the joint running speed;
[0065] S8: Determine whether the joint limit avoidance angle output value exceeds the speed limit, and output the actual output angle of the joint according to the determination result.
[0066] Furthermore, step S8 specifically includes:
[0067] Determine whether the joint limit avoidance angle output value exceeds the speed limit.
[0068] If yes, then the joint angle corresponding to the current inverse solution result is output as the actual output angle of the joint;
[0069] If not, the joint limit avoidance angle output value is used as the actual joint output angle.
[0070] Furthermore, step S7 specifically includes:
[0071] Calculate the distance d from the current position of the redundant robot arm to the joint limit center point;
[0072] Estimate the number of control cycles n required to run at the joint speed to the joint limit center point;
[0073] Reduce the joint limit avoidance increment by n times, and apply it to the current inverse solution result for output to obtain the joint angle output value.
[0074] Furthermore, the calculation formula of the joint running speed is as follows:
[0075]
[0076] in, is the joint running speed, q solve is the current inverse solution result, q ref is the current joint angle, and dt is the control period.
[0077] Furthermore, the calculation formula for the distance d from the current position of the redundant robotic arm to the joint limit center point is as follows:
[0078]
[0079] In formula (2), q max Indicates the maximum limit of the joint, q min Indicates the minimum limit of the joint. Further, the calculation formula of the control cycle number n is as follows:
[0080]
[0081] Where max(·) is the vector maximum operator (i.e., the maximum value of the vector), and round(·) is the rounding operator (i.e., the integer value).
[0082] Furthermore, the calculation formula of the joint angle output value is as follows:
[0083]
[0084] Where Δq is the joint limit avoidance increment, Output value of joint limit avoidance angle.
[0085] Furthermore, the redundant parameters are solved and a set of inverse solutions is selected as the current inverse solution result, including: using spatial geometric relationships to solve multiple sets of inverse solutions for redundant robotic arms, and selecting a set of inverse solutions that is closest to the current joint angle from the multiple sets of inverse solutions to obtain the current inverse solution result.
[0086] Furthermore, the calculation formula of the joint limit avoidance increment is as follows:
[0087]
[0088] Where k is the avoidance velocity coefficient, C(q) is the variable weight function matrix, which is in the form of a diagonal matrix, consisting of the variable weight function c of each joint i (q i )composition, is the null space projection matrix, is the gradient of the task function H(q) for joint limit avoidance.
[0089] Furthermore, the variable weight function c of each joint i (q i ) is calculated as follows:
[0090] c i (q i )=Acos(π-πt i )+A+1 ; (6)
[0091] Where A is the maximum escape ratio, t i is the damping coefficient.
[0092] A specific embodiment of the present invention is given below based on the joint limit avoidance method of the redundant mechanical arm, as follows:
[0093] The relationship between the end effector velocity and joint angular velocity of the robot arm is described by the following formula:
[0094]
[0095] In formula (7), is the velocity vector in the task space, Usually consists of three linear velocity vectors and three angular velocity vectors. represents a set of real numbers, m is the dimension of the task space, are the joint angles and angular velocities of the robot arm, w is the joint degree of freedom of the robotic arm, and J(q) is the Jacobian matrix of the robotic arm. m×w indicates that the matrix has m rows and w columns.
[0096] For a redundant robotic arm, m < w. Therefore, for a given there will be infinitely many solutions for the joint angular velocity. Usually, we only take the minimum-norm solution of Equation (7), that is:
[0097]
[0098] In Equation (8), is the Moore-Penrose pseudoinverse of the Jacobian matrix J (the Moore-Penrose pseudoinverse is often used to solve or simplify the minimum-norm least-squares solution of inconsistent linear equations). This solution provides the
[0099] Since the redundant robotic arm has non-zero homogeneous solutions, adding the non-zero homogeneous solutions to the right side of Equation (8) gives:
[0100]
[0101] In the formula, I is the identity matrix, is the null space projection matrix, is an arbitrary vector, belongs to the null space of the Jacobian matrix J and is called the null space self-motion.
[0102] The present invention utilizes the null space self-motion characteristics of the redundant robotic arm to achieve joint limit avoidance. The specific steps of this embodiment are as follows:
[0103] Step 1: Obtain the current joint angles, current pose data, and pose data to be solved of the redundant robotic arm;
[0104] Step 2: Determine the pose increment based on the current joint angles, current pose data, and pose data to be solved;
[0105] Step 3. Determine the redundant parameters based on the posture increment and the current joint angle, calculate multiple sets of inverse solutions of the redundant manipulator according to the spatial geometric relationship, and select a set of analytical inverse solutions closest to the current joint angle. In this step, redundant parameters are a term in the inverse kinematics settlement process of the redundant manipulator, which refers to the joint angle or arm angle of the redundant manipulator. Among them, the arm angle is the angle between the arm plane formed by the shoulder joint, elbow joint and wrist joint of the redundant manipulator and the preset reference plane. For example, when the preset reference plane is a vertical plane, the above arm angle is the angle between the arm plane and the vertical plane. After the redundant parameters are determined, 8 sets of inverse solution angles will be calculated, and then a solution closest to the current joint angle will be selected from these 8 groups as the current inverse solution result.
[0106] Step 4: Calculate the corresponding Jacobian matrix according to the current joint angle, and solve the null space projection matrix N corresponding to the current robot arm. It should be noted that the calculation of the null space in the present invention is achieved by means of a projection matrix, and it can also be achieved by directly solving the null space of the Jacobian matrix.
[0107] Step 5: Calculate the variable weight function of each joint position, and use the variable weight function and the null space projection matrix to calculate the joint limit avoidance increment of each joint position. The specific avoidance control law is as follows:
[0108]
[0109] In formula (5), k is the avoidance speed coefficient, which can be used to avoid the maximum speed or adjust the maximum speed of the avoidance action, Δq is the joint limit avoidance increment (the joint angle increment generated by avoiding the limit), dt is the control period, is the gradient of the task function H(q) for joint limit avoidance, C(q) is the variable weight function matrix, which is in the form of a diagonal matrix, consisting of the variable weight function c of each joint i (q i ), namely:
[0110]
[0111] In formula (10), q i (i=1,2,…,s) is the angle of joint i, and s is the number of joints.
[0112] In order to obtain a trajectory with good configuration repeatability, the task function H(q) for joint limit avoidance is designed as:
[0113]
[0114] In formula (11), q i,max is the maximum limit of joint i, q i,min is the minimum limit of joint i.
[0115] Configuration repeatability refers to the similarity of the configuration of the redundant robot arm when the end of the robot arm repeatedly moves along the same trajectory. The higher the similarity, the better the repeatability.
[0116] Variable weight function c i (q i ) is designed to:
[0117] c i (q i )=Acos(π-πt i )+A+1; (6)
[0118] In formula (6), A is the maximum escape ratio, t i is the position-dependent evasive action activation, which is defined as follows:
[0119]
[0120] In formula (12), q i,damp+ is the position where the damping parameters are activated in the positive direction of motion, q i,damp- is the position where the damping parameter is activated in the negative direction of motion and satisfies Δq damp =q i,max -q i,damp+ =q i,damp- -q i,min , Δq damp is the damping area size.
[0121] Taking joint 1 as an example, assuming q 1,max =120deg,q 1,min =-120deg, Δq damp =50 degrees, so q i,damp- =-70deg, q i,damp+ =70deg, damping coefficient curve Figure 2 As shown, the variable weight coefficient curve is as follows Figure 3 As shown, combined Figure 2 and Figure 3 It can be seen that when the joint is outside the damping zone, the activation amount is 0 and the weight is 1, the boundary avoidance action will not be strengthened, and the constant weight mode will be maintained; when the joint enters the damping zone, the activation amount is not 0, the closer to the boundary, the greater the activation amount, the weight increases with the increase of the activation amount, the boundary avoidance action is strengthened, and the robotic arm produces a larger amplitude boundary avoidance action.
[0122] Furthermore, in order to solve the problem of different limit avoidance effects at different speeds, the present invention further designs speed adaptation:
[0123] Step 6: Calculate the joint running speed based on the current joint angle and the current inverse solution result; specifically: based on the current joint angle q ref And the current inverse solution result q solve Estimating joint speed q solve is the inverse solution selected in step 3, dt is the control period, and the estimation is performed as follows:
[0124]
[0125] Step 7: Based on the current inverse solution result, joint limit avoidance increment and joint running speed, determine the joint limit avoidance angle output value. The specific process is as follows:
[0126] 7.1. Calculate the distance d from the current position of the robot arm to the center point of the joint limit according to the following formula:
[0127]
[0128] In formula (2), q max Indicates the maximum limit of the joint, q min Indicates the minimum limit of the joint.
[0129] 7.2. Estimate the number of control cycles n (number of control cycles) required to run to the joint limit center point at the current motion speed using equations (1) and (2), where:
[0130]
[0131] In formula (3), max(·) is the vector maximum operator (i.e., the maximum value of the vector), and round(·) is the rounding operator (i.e., the integer value).
[0132] 7.3, the joint limit avoidance increment calculated by the zero space avoidance control law of formula (5) is reduced by n times, and acts on the inverse solution calculation result to output, that is, the joint limit avoidance angle output value Calculate as follows:
[0133]
[0134] Step 8: Determine whether the joint limit avoidance angle output value is overspeed, and output the actual joint output angle according to the judgment result. The specific process is: In order to prevent overspeed caused by zero space avoidance, further processing can be performed to estimate the joint limit avoidance angle output value Whether it is overspeeding, if it is overspeeding, the implementation of the avoidance action is not considered, and the joint angle corresponding to the current inverse solution result is output as the actual output angle of the joint; if not, the joint limit avoidance angle output value is used as the actual output angle of the joint, and the calculation is shown in the following formula:
[0135]
[0136] In the formula, is the maximum joint velocity, q out The actual output angle of the joint.
[0137] For the comparison of the limiting avoidance effect of the present invention and the conventional solution, see Figure 4-10 : Taking the SRS configuration 7-DOF manipulator as an example, the manipulator moves in a straight line from point A to point B and then returns to point A:
[0138] The position of point A is X=225mm, Y=115mm, Z=350mm, RX=0rad, RY=0rad, Rz=0.524rad;
[0139] The position of point B is X=225mm, Y=115mm, Z=750mm, RX=0rad, RY=0rad, Rz=0.524rad;
[0140] The rod length parameters are Lbs = 245 mm, Lse = 256 mm, Lew = 210 mm, Lwt = 144 mm;
[0141] Among them, rad is radians, X, Y, and Z represent the position of the origin of the tool coordinate system in the base coordinate system, Rx, Ry, and Rz describe the orientation relationship between the two coordinate systems; Lbs is the length from the origin of the base coordinate system to the origin of the shoulder joint coordinate system, Lse is the length from the origin of the shoulder joint coordinate system to the origin of the elbow joint coordinate system, Lew is the length from the origin of the elbow joint coordinate system to the origin of the wrist joint coordinate system, and Lwt is the length from the origin of the wrist joint coordinate system to the origin of the tool coordinate system. The limits of each joint are as follows:
[0142] The limit of joint 1 is -178~178;
[0143] The limit of joint 2 is -130~130;
[0144] The limit of joint 3 is -178~178;
[0145] The limit of joint 4 is -135~135;
[0146] The limit of joint 5 is -178~178;
[0147] The limit of joint 6 is -128~128;
[0148] The position limit of joint 7 is -360 to 360;
[0149] The damping zone size is set to 10 degrees;
[0150] from Figure 4-10It can be seen that the sixth joint is compared with the control group ( Figure 4 ), the avoidance effect is better. In the 6th joint, the control exceeds the joint limit at the 6800th sampling point, but the present invention basically maintains -120 degrees after the 6000th sampling point under the action of the avoidance algorithm, and avoids the limit very well. The avoidance effect of other joints is similar to that of the control because other joints are far away from the joint limit ( Figure 5-10 ), and the sixth joint is close to the joint limit. Therefore, it can be seen from the comparison that the conventional joint limit avoidance method cannot avoid the joint limit well at some points, while the method proposed in the present invention can well achieve the avoidance of joint limit.
[0151] In addition, in the above steps, the present invention can also be implemented by modifying the optimization function or variable weight function, simplifying the calculation process, modifying the calculation order, modifying the incremental constraint method, and modifying the speed adaptation. The present invention not only solves the problem that the elbow movement of the robot arm cannot be predicted during the movement and the joint limit cannot achieve effective avoidance, but also further designs a speed-adaptive avoidance method to solve the problem of joint overspeed caused by the different avoidance action amplitudes of the robot at different operating speeds. The present invention has the following advantages:
[0152] 1. For the first time, a method was designed to make elbow motion deterministic and effectively avoid joint limitations;
[0153] 2. The design method of the present invention is universal for redundant robotic arms;
[0154] 3. The algorithm has small computational complexity and good real-time performance.
[0155] Those skilled in the art will appreciate that the embodiments of the present invention may be provided as methods, systems, etc. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects.
[0156] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.
Claims
1. A method for avoiding joint limit of a redundant robotic arm, characterized in that: include: S1: Obtain the current joint angle, current posture data and posture data to be solved of the redundant robot arm; S2: determining a posture increment according to the current joint angle, the current posture data and the posture data to be solved; S3: determining redundant parameters based on the posture increment and the current joint angle, solving the redundant parameters and selecting a set of inverse solutions as the current inverse solution result; S4: Calculate the corresponding Jacobian matrix according to the current joint angle, and solve the null space projection matrix corresponding to the current robot arm configuration through the Jacobian matrix; S5: Calculate the variable weight function of each joint position, and calculate the joint limit avoidance increment of each joint position using the variable weight function and the null space projection matrix; S6: Calculating the joint running speed according to the current joint angle and the current inverse solution result; S7: Determine a joint limit avoidance angle output value based on the current inverse solution result, the joint limit avoidance increment and the joint running speed; S8: Determine whether the joint limit avoidance angle output value is overspeed, and output the actual output angle of the joint according to the judgment result.
2. The joint limit avoidance method of a redundant mechanical arm according to claim 1, characterized in that: Step S8 specifically includes: Determine whether the joint limit avoidance angle output value exceeds the speed limit, If so, the joint angle corresponding to the current inverse solution result is output as the actual output angle of the joint; if not, the joint limit avoidance angle output value is used as the actual output angle of the joint.
3. The joint limit avoidance method of a redundant mechanical arm according to claim 1, characterized in that: Step S7 specifically includes: Calculate the distance d from the current position of the redundant robot arm to the joint limit center point; Estimating the number of control cycles n of running to the joint limit center point at the joint running speed; The joint limit avoidance increment is reduced by n times, and is applied to the current inverse solution result for output to obtain a joint angle output value.
4. The joint limit avoidance method of a redundant mechanical arm according to claim 3, characterized in that: The calculation formula of the joint running speed is as follows: in, is the joint running speed, q solve is the current inverse solution result, q ref is the current joint angle, and dt is the control period.
5. The joint limit avoidance method of a redundant mechanical arm according to claim 4, characterized in that: The calculation formula of the distance d from the current position of the redundant manipulator to the joint limit center point is as follows: Among them, q max Indicates the maximum limit of the joint, q min Indicates the minimum limit of the joint.
6. The joint limit avoidance method of a redundant mechanical arm according to claim 5, characterized in that: The calculation formula of the control cycle number n is as follows: Where max(·) is the vector maximum operator, and round(·) is the integer operator.
7. The joint limit avoidance method of a redundant mechanical arm according to claim 6, characterized in that: The calculation formula of the joint angle output value is as follows: Where Δq is the joint limit avoidance increment, Output value of joint limit avoidance angle.
8. The method for avoiding joint limit of a redundant mechanical arm according to claim 1, characterized in that: The redundant parameters are solved and a set of inverse solutions are selected as the current inverse solution result, including: using spatial geometric relationships to solve multiple sets of inverse solutions of redundant robotic arms, and selecting a set of inverse solutions that is closest to the current joint angle from the multiple sets of inverse solutions to obtain the current inverse solution result.
9. The method for avoiding joint limit of a redundant mechanical arm according to claim 7, characterized in that: The calculation formula of the joint limit avoidance increment is as follows: Where k is the avoidance velocity coefficient, C(q) is the variable weight function matrix, which is in the form of a diagonal matrix, consisting of the variable weight function c of each joint i (q i )composition, is the null space projection matrix, ▽H is the gradient of the task function H(q) for joint limit avoidance.
10. The method for avoiding joint limit of a redundant mechanical arm according to claim 9, characterized in that: The variable weight function c of each joint i (q i ) is calculated as follows: c i (q i )=Acos(π-πt i )+A+1; Where A is the maximum escape ratio, t i is the damping coefficient.
Citation Information
Cited By
Mobile robot control method and system based on fault prediction
CN120461449A
Mobile robot control method and system based on fault prediction
CN120461449B
