A method and system for inverse kinematics control of a srs configuration robotic arm

Abstract

The application discloses a kind of SRS configuration mechanical arm inverse motion control method and system, method includes: establishing the function relationship between each joint angle of several joints of seven degrees of freedom mechanical arm and arm angle, determine the stationary point of each function relationship, arm angle is defined as the rotation angle of actual arm plane relative to reference arm plane around shoulder to wrist line;Obtain the intersection of each function relationship and the preset angle limit line of corresponding joint;According to the position information of stationary point and intersection, calculate the feasible arm angle interval of mechanical arm in the process of motion, the feasible arm angle interval is the arm angle value range for making all joint angles be in its corresponding angle limit range;According to the feasible arm angle interval, select target arm angle, and generate joint angle control instruction of mechanical arm based on target arm angle.Through systematic analysis of the function relationship between joint angle and arm angle, the feasible arm angle interval is accurately calculated, and the efficient and reliable online avoidance of seven degrees of freedom mechanical arm joint motion limit is realized.

Description

Technical Field

[0001] This invention relates to the field of robotic arm control technology, and in particular to a method and system for reverse motion control of an SRS-configured robotic arm. Background Technology

[0002] Seven-DOF robotic arms, with their humanoid design, exhibit exceptional mobility in complex, unstructured environments. Compared to traditional six-DOF robotic arms, the additional seventh degree of freedom introduces redundancy, allowing the arm to self-move within its zero-space while maintaining the end effector's pose. This characteristic offers possibilities for optimizing the arm's motion performance, such as avoiding joint limitations, navigating obstacles, and optimizing energy consumption; however, it also significantly increases the complexity of solving its inverse kinematics.

[0003] To solve the inverse kinematics problem of a seven-DOF robotic arm, a common analytical approach is to reduce the dimensionality of the problem to a six-DOF problem by setting a redundant parameter. Among these, using the arm angle as a redundant parameter is a widely used strategy for unbiased SRS (shoulder-elbow-wrist) robotic arms. The arm angle parameter has good geometric intuition, clearly describing the rotational motion of the elbow around the shoulder-wrist line, facilitating direct intervention and optimization of the robotic arm's configuration.

[0004] After obtaining multiple sets of solutions for inverse kinematics using arm angle parameters, selecting the optimal solution becomes crucial to determining the robotic arm's motion performance. Existing solution selection strategies primarily focus on the continuity of joint motion and the avoidance of joint limitations. The most intuitive method is to iterate through discrete arm angle values ​​to obtain all corresponding joint solutions, and then select the optimal solution by designing an evaluation function. Another approach is to apply intelligent optimization algorithms such as particle swarm optimization to perform adaptive search within the solution space to find solutions that satisfy the optimization objective. The introduction of the arm angle concept makes it possible to select solutions by directly intervening in the arm angle based on the optimization objective.

[0005] However, these existing technical solutions all have significant limitations. The ergonomic method is computationally inefficient, with computation time increasing significantly with increasing solution accuracy. Furthermore, the choice of distance step size is difficult to control, easily leading to jagged, uneven joint trajectories. While intelligent optimization algorithms facilitate the addition of complex optimization metrics, they inherently rely on random search, resulting in high computational overhead, slow convergence, and the risk of getting trapped in local optima. More importantly, current arm angle-based optimization strategies lack a comprehensive analysis of the functional relationship between joint angles and arm angles. They fail to systematically reveal the intrinsic connection between different curve shapes (such as the number of stationary points and singularities) and joint limitations, and lack a complete method for solving feasible arm angle intervals for various possible curve shapes. This results in a lack of an efficient and reliable solution for selecting appropriate arm angles online to strictly avoid joint motion limits when performing Cartesian space motion at the end effector of a robotic arm. Summary of the Invention

[0006] The purpose of this invention is to provide an inverse motion control method and system for an SRS-configured robotic arm. Through systematic analysis of the relationship between joint angles and arm angle functions, feasible arm angle intervals are accurately calculated, achieving efficient and reliable online avoidance of the joint motion limits of a seven-degree-of-freedom robotic arm.

[0007] To address the aforementioned technical problems, a first aspect of this invention provides an inverse motion control method for an SRS-configured robotic arm, comprising the following steps:

[0008] Establish the functional relationship between several joint angles of a seven-degree-of-freedom robotic arm and the arm angle, and determine the stationary point of each of the aforementioned functional relationships. The arm angle is defined as the rotation angle of the actual arm plane relative to the reference arm plane around the line connecting the shoulder and wrist.

[0009] Obtain the intersection point of each of the aforementioned functional relationships and the preset angle limit line of the corresponding joint;

[0010] Based on the position information of the stationary point and the intersection point, the feasible arm angle range during the movement of the robotic arm is calculated. The feasible arm angle range is the range of arm angle values ​​that ensures that all joint angles are within their corresponding angle limits.

[0011] Select a target arm angle based on the feasible arm angle range, and generate joint angle control commands for the robotic arm based on the target arm angle.

[0012] Furthermore, establishing the functional relationship between the joint angles of the seven-degree-of-freedom robotic arm and the arm angle includes:

[0013] Based on the configuration parameters of the robotic arm, the arm plane with the elbow pointing vertically upward when the arm angle is zero degrees is determined as the reference arm plane, and the virtual joint angle corresponding to the reference arm plane is calculated.

[0014] The rotation matrix corresponding to the actual arm plane obtained by rotating the reference arm plane around the line connecting the shoulder and wrist by the arm angle is calculated using the Rodriguez rotation transformation formula.

[0015] From the rotation matrix corresponding to the actual arm plane, the functional expressions of each joint angle with respect to the arm angle are extracted;

[0016] Among them, the function expression of some joint angles is in arctangent form, which includes sine and cosine functions, while the function expression of other joint angles is in arccosine form.

[0017] Further, the step of calculating the feasible arm angle range of the robotic arm during its movement based on the position information of the stationary point and the intersection point includes:

[0018] Based on the number of the stationary points and the relative positional relationship between the intersection point and the stationary point, determine the distribution pattern of the function curve of each joint angle and arm angle relative to its corresponding angle limit line;

[0019] Based on the distribution pattern, determine the arm angle sub-interval that makes the corresponding joint angle within its angle limit range;

[0020] The feasible arm angle interval is obtained by taking the intersection of the arm angle sub-intervals corresponding to all joints.

[0021] Furthermore, the distribution pattern includes:

[0022] The function curve has two stationary points, and the maximum point is lower than the upper limit of the joint angle and the minimum point is higher than the lower limit of the joint angle.

[0023] The function curve has two stationary points, with the maximum point being higher than the upper limit of the joint angle and the minimum point being lower than the lower limit of the joint angle.

[0024] The function curve does not have any stationary points;

[0025] The function curve has a stationary point, and the function value undergoes a step change at the stationary point.

[0026] Furthermore, when the function curve has a stationary point and the function value undergoes a step change at the stationary point, determining the arm angle sub-interval that keeps the corresponding joint angle within its angular limit range based on the distribution pattern includes:

[0027] Calculate the left and right limits of the function at the stationary point;

[0028] When both the left limit value and the right limit value are within the angle limit range of the corresponding joint, the stationary point is excluded from the arm angle sub-range.

[0029] When the left limit value and / or the right limit value exceed the angle limit range, the arm angle intervals that meet the joint angle limit requirements on both sides of the stationary point are selected according to the intersection of the function relationship and the angle limit line, and the selected arm angle intervals are included in the arm angle sub-intervals.

[0030] Furthermore, prior to the step of establishing the functional relationship, the following steps are also included:

[0031] Set global configuration parameters, which are used to specify the target angle symbol combination for joints two, four, and six;

[0032] Based on the target angle symbol combination corresponding to the global configuration parameters, a functional relationship between each joint angle and arm angle is established, and the functional relationship corresponds to a set of joint angle solutions uniquely determined by the target angle symbol combination.

[0033] Further, the step of selecting the target arm angle based on the feasible arm angle range includes:

[0034] Based on the historical arm angle value of the previous control cycle and the upper and lower boundaries of the currently calculated feasible arm angle interval, the target arm angle value of the current control cycle is calculated through the bounded cosine function mapping relationship.

[0035] Furthermore, the bounded cosine function mapping relationship is as follows:

[0036]

[0037] Where ψ(t) is the target arm angle value of the current control cycle, and ψ(t-1) is the historical arm angle value of the previous control cycle. all_upper and ψ all_lower Let be the upper and lower boundaries of the currently calculated feasible arm angle interval, respectively; K is a constant controlling the repulsion strength; and α is a constant controlling the initiation position of the repulsion reaction.

[0038] Accordingly, a second aspect of the present invention provides an SRS configuration robotic arm inverse motion control system, which controls the SRS configuration robotic arm based on the above-described SRS configuration robotic arm inverse motion control method. The control system includes:

[0039] The stationary point acquisition module is used to establish the functional relationship between several joint angles of the seven-degree-of-freedom robotic arm and the arm angle, and to determine the stationary point of each functional relationship. The arm angle is defined as the rotation angle of the actual arm plane relative to the reference arm plane around the line connecting the shoulder and wrist.

[0040] The intersection point acquisition module is used to acquire the intersection point of each of the aforementioned functional relationships and the preset angle limit line of the corresponding joint;

[0041] The arm angle calculation module is used to calculate the feasible arm angle range of the robotic arm during the movement process based on the position information of the stationary point and the intersection point. The feasible arm angle range is the range of arm angle values ​​that makes all joint angles within their corresponding angle limits.

[0042] The instruction generation module is used to select a target arm angle based on the feasible arm angle range, and generate joint angle control instructions for the robotic arm based on the target arm angle.

[0043] Accordingly, a third aspect of the present invention provides an electronic device, including: at least one processor; and a memory connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to cause the at least one processor to perform the above-described SRS configuration robotic arm inverse motion control method.

[0044] Accordingly, a fourth aspect of the present invention provides a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the above-described SRS configuration robotic arm inverse motion control method.

[0045] The above-described technical solutions of the embodiments of the present invention have the following beneficial technical effects:

[0046] 1. By systematically constructing the functional relationship between joint angles and arm angles and analyzing its curve characteristics (such as stationary points and intersections with limit lines), the feasible arm angle range in which all joints do not exceed the limits can be accurately calculated. This fundamentally solves the problem of joints easily exceeding limits and trajectory changes in the Cartesian space motion of a seven-degree-of-freedom robotic arm, ensuring the reliability and safety of the motion.

[0047] 2. By introducing global configuration parameters to pre-lock the desired robotic arm configuration, this invention avoids the huge computational overhead of traditional methods that require traversing all eight sets of inverse solutions before screening, and also overcomes the computational redundancy and local convergence risk brought about by the random search of intelligent optimization algorithms, making online real-time calculation efficient and the results deterministic.

[0048] 3. The adaptive arm angle selection strategy based on the bounded cosine function can smoothly generate the optimal arm angle at the current moment within the feasible arm angle range based on the arm angle value at the previous moment. Its output value is naturally bounded, thus effectively avoiding arm angle jumps and sudden changes in joint velocity, and finally generating a continuous, smooth and natural joint trajectory. Attached Figure Description

[0049] Figure 1 This is a flowchart of the SRS configuration robotic arm inverse motion control method provided in this embodiment of the invention;

[0050] Figure 2 This is a schematic diagram of the arm-shaped angle model provided in an embodiment of the present invention;

[0051] Figure 3 This is a schematic diagram of the virtual arm planar configuration provided in an embodiment of the present invention;

[0052] Figure 4a This is a schematic diagram of the curves for the angle functions of joints 1 / 3 / 5 / 7-arms provided in this embodiment of the invention when there are two stationary points. Figure 1 ;

[0053] Figure 4b This is a schematic diagram of the curves for the angle functions of joints 1 / 3 / 5 / 7-arms provided in this embodiment of the invention when there are two stationary points. Figure 2 ;

[0054] Figure 4c This is a schematic diagram of the curves for the angle functions of joints 1 / 3 / 5 / 7-arms provided in this embodiment of the invention when there are two stationary points. Figure 3 ;

[0055] Figure 4d This is a schematic diagram of the curve of the angle function of joint 1 / 3 / 5 / 7-arm with two stationary points provided in the embodiment of the present invention;

[0056] Figure 5a This is a schematic diagram of the curves of the angle functions of joints 1 / 3 / 5 / 7-arms provided in this embodiment of the invention when there are no stationary points. Figure 1 ;

[0057] Figure 5b This is a schematic diagram of the curves of the angle functions of joints 1 / 3 / 5 / 7-arms provided in this embodiment of the invention when there are no stationary points. Figure 2 ;

[0058] Figure 6a This is a schematic diagram of the curve when the angle function of joint 1 / 3 / 5 / 7-arm has a stationary point, as provided in the embodiment of the present invention. Figure 1 ;

[0059] Figure 6b This is a schematic diagram of the curve when the angle function of joint 1 / 3 / 5 / 7-arm has a stationary point, as provided in the embodiment of the present invention. Figure 2 ;

[0060] Figure 7a This is a schematic diagram of the curve of the joint 2 / 6-arm angle function with two stationary points provided in the embodiment of the present invention. Figure 1 ;

[0061] Figure 7b This is a schematic diagram of the curve of the joint 2 / 6-arm angle function with two stationary points provided in the embodiment of the present invention. Figure 2 ;

[0062] Figure 7cThis is a schematic diagram of the curve of the joint 2 / 6-arm angle function with two stationary points provided in the embodiment of the present invention. Figure 3 ;

[0063] Figure 8 This is a schematic diagram of the curve when the angle function of joint 2 / 6-arm has a singularity, provided in an embodiment of the present invention;

[0064] Figure 9a yes Figure 4a Analysis diagram Figure 1 ;

[0065] Figure 9b yes Figure 4a Analysis diagram Figure 2 ;

[0066] Figure 9c yes Figure 4a Analysis diagram Figure 3 ;

[0067] Figure 10a yes Figure 4b Analysis diagram Figure 1 ;

[0068] Figure 10b yes Figure 4b Analysis diagram Figure 2 ;

[0069] Figure 10c yes Figure 4b Analysis diagram Figure 3 ;

[0070] Figure 11a yes Figure 4c Analysis diagram Figure 1 ;

[0071] Figure 11b yes Figure 4c Analysis diagram Figure 2 ;

[0072] Figure 11c yes Figure 4c Analysis diagram Figure 3 ;

[0073] Figure 11d yes Figure 4c Analysis diagram four;

[0074] Figure 12a yes Figure 4d Analysis diagram Figure 1 ;

[0075] Figure 12b yes Figure 4d Analysis diagram Figure 2 ;

[0076] Figure 12c yes Figure 4d Analysis diagram Figure 3 ;

[0077] Figure 12d yes Figure 4d Analysis diagram four;

[0078] Figure 13 yes Figure 5a A schematic diagram of classification analysis;

[0079] Figure 14a yes Figure 5b Analysis diagram Figure 1 ;

[0080] Figure 14b yes Figure 5b Analysis diagram Figure 2 ;

[0081] Figure 14c yes Figure 5b Analysis diagram Figure 3 ;

[0082] Figure 15a This is a schematic diagram of the joint trajectory in the fixed arm angle method;

[0083] Figure 15b This is a schematic diagram of the optimized joint trajectory in an embodiment of the present invention;

[0084] Figure 16 This is a block diagram of the SRS configuration robotic arm reverse motion control system module in an embodiment of the present invention.

[0085] Figure label:

[0086] 1. Stationary point acquisition module; 2. Intersection point acquisition module; 3. Arm angle calculation module; 4. Instruction generation module. Detailed Implementation

[0087] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments and the accompanying drawings. It should be understood that these descriptions are merely exemplary and not intended to limit the scope of the invention. Furthermore, descriptions of well-known structures and techniques are omitted in the following description to avoid unnecessarily obscuring the concept of the invention.

[0088] For a redundant manipulator with an SRS configuration, there are countless inverse joint solutions corresponding to the same end-effector pose. The null space characteristic of this infinite set of solutions is related to the selection of the arm angle; different arm angles correspond to different solution spaces. There exists a functional relationship between the arm angles and joint angles of the SRS manipulator that only has discontinuities of the first kind (jump discontinuities and removable discontinuities). By analyzing the intersection of the function curves with the upper and lower limit lines of each joint through the stationary points of these functions, an arm angle interval that ensures the joints are within the limit range can be obtained. Then, a better arm angle is selected within this arm angle interval using a preset strategy. Finally, this arm angle is used to perform inverse kinematics calculations to obtain the inverse solution that satisfies the joint limits.

[0089] Please refer to Figure 1 To achieve the above objectives, a first aspect of the present invention provides an inverse motion control method for an SRS-configured robotic arm, comprising the following steps:

[0090] Step S100: Establish the functional relationship between several joint angles of the seven-degree-of-freedom robotic arm and the arm angle, and determine the stationary point of each functional relationship. The arm angle is defined as the rotation angle of the actual arm plane relative to the reference arm plane around the line connecting the shoulder and wrist.

[0091] This paper establishes the functional relationship between the joint angles and arm angles of a seven-DOF robotic arm and determines their stationary points. First, based on the DH parameters and other configuration definitions of the robotic arm, the arm plane corresponding to the configuration where the elbow is vertically upward when the arm angle is zero degrees is determined as the reference arm plane, and the angle values ​​of each virtual joint under this specific configuration are calculated. Then, using the Rodrigues rotation transformation formula, the rotation matrix corresponding to the actual arm plane obtained by rotating the reference arm plane around the vector connecting the shoulder S to the wrist W by an arbitrary arm angle ψ is calculated. Starting from the rotation matrix of this actual arm plane, by simultaneously solving the system of equations with the forward kinematics of the robotic arm and performing algebraic elimination, the explicit functional expressions of the angles of joints 1, 2, 3, 5, 6, and 7 with respect to the arm angle ψ are finally analyzed. These functional expressions take two forms: for joints 1, 3, 5, and 7, the expression is an arctangent function in which both the numerator and denominator contain a linear combination of the sine function sin(ψ) and the cosine function cos(ψ); for joints 2 and 6, the expression is an inverse cosine function in which both the numerator and denominator contain a linear combination of sin(ψ) and cos(ψ). After obtaining these functional relationships, it is necessary to further solve for all stationary points of each function within its domain by taking the derivative and setting the derivative to zero.

[0092] Step S200: Obtain the intersection point of each functional relationship and the preset angle limit line of the corresponding joint.

[0093] To find the coordinates of all intersection points between the curve of each joint angle function and the upper and lower limit horizontal lines representing its physical motion limits, the joint angle value needs to be set to its upper limit value q. max Or lower limit q min And substitute it into the functional relationship θ obtained in step S100 i(ψ) In the reverse process, the arm angle value ψ is calculated. This process involves solving trigonometric equations for sin(ψ) and cos(ψ), and the number of solutions (0, 1, or 2) depends on the relative position of the function curve and the limit line. All these solutions represent the arm angle values ​​at the intersection of the function curve and the limit line, which are the key boundary points for subsequent feasibility interval determination.

[0094] Step S300: Based on the position information of the stationary point and the intersection point, calculate the feasible arm angle range during the movement of the robotic arm. The feasible arm angle range is the range of arm angle values ​​that ensures all joint angles are within their corresponding angle limits.

[0095] First, the function curve shape of each joint needs to be classified and determined by considering the number and type of stationary points (maximum or minimum points) of each joint function, as well as the relative distribution of their intersections with the upper and lower limit lines. Typical shapes include, but are not limited to: shapes with two stationary points and both extreme points within the limit range; shapes with two stationary points and both extreme points exceeding the limit range; monotonic shapes with no stationary points; and singular shapes with only one stationary point where the function value jumps by π radians. For each identified shape, based on the comparison between its stationary point function value and the joint limit value, a specific interval selection rule is used to determine one or more arm angle sub-intervals that ensure the joint angle does not exceed the limit. Finally, the logical intersection of the allowable arm angle sub-intervals calculated for each joint is taken to obtain the globally feasible arm angle interval ψ that simultaneously satisfies all joint limit constraints. all .

[0096] Step S400: Select the target arm angle according to the feasible arm angle range, and generate joint angle control commands for the robotic arm based on the target arm angle.

[0097] After obtaining the globally feasible arm angle interval [ψ] all_lower ,ψ all_upperAfterwards, a specific arm angle value ψ(t) needs to be selected for the inverse kinematics calculation of the current control cycle. The selection strategy needs to balance safety and motion smoothness: using the arm angle value ψ(t-1) used in the previous control cycle as input, and its relative distance to the boundary of the feasible interval as a variable, a designed bounded cosine function is used for mapping calculation. This function can generate an adjustment amount biased towards the center of the feasible interval, ensuring that the output value ψ(t) is strictly within the feasible interval, while its smoothness ensures the continuity of arm angle changes. Finally, the selected target arm angle ψ(t) is substituted into the joint angle-arm angle function relationship established in step S100 to calculate the angle values ​​of all joints, and then converted into control commands executed by the drive motor.

[0098] By establishing the precise functional relationship between joint angles and arm angles and using their mathematical properties for systematic analysis, the feasible arm angle working range that strictly satisfies all joint angle limit constraints when the robotic arm executes a Cartesian space trajectory can be calculated online and automatically. Based on this, a smooth adaptive strategy is adopted to select the optimal arm angle, ultimately generating a safe, continuous, and abrupt high-quality joint trajectory. This fundamentally solves the technical problems of joints easily exceeding limits during trajectory tracking in seven-degree-of-freedom redundant robotic arms, as well as the low computational efficiency or unstable results of traditional solution selection methods.

[0099] Furthermore, the establishment of the functional relationship between the joint angles of the seven-degree-of-freedom robotic arm and the arm angle in step S100 includes:

[0100] Step S110: Based on the configuration parameters of the robotic arm, determine the arm plane with the elbow pointing vertically upward when the arm angle is zero degrees as the reference arm plane, and calculate the virtual joint angle corresponding to the reference arm plane.

[0101] For a seven-DOF robotic arm with an SRS configuration, the reference arm plane is used when the elbow is vertically upward in the initial configuration. This plane is determined by the positions of the shoulder, elbow, and wrist. Using the robotic arm's DH parameters and the current end effector pose, the vector from the base coordinate system to the wrist can be calculated. Then, based on geometric relationships, the virtual joint angles, including the angles of virtual joints 1, 2, 3, and 4, are solved. These virtual joint angles characterize the robotic arm configuration in the reference arm plane, providing a basis for subsequent arm angle transformations.

[0102] Step S120: Using the Rodriguez rotation transformation formula, calculate the rotation matrix corresponding to the actual arm plane obtained by rotating the reference arm plane around the line connecting the shoulder and wrist by an arm angle. The function expression for some joint angles is an arctangent form containing sine and cosine functions, while the function expression for other joint angles is an inverse cosine form.

[0103] Based on the virtual joint angles obtained in the reference arm plane, the rotation matrix in the reference arm plane is calculated using forward kinematics. Then, using the vector from the shoulder to the wrist as the rotation axis, the rotation matrix in the reference arm plane is transformed using the Rodriguez rotation formula to obtain the rotation matrix in the actual arm plane. This rotation matrix can be decomposed into a linear combination of sine and cosine functions of the arm angles. The functional expressions for some joint angles are represented in arctangent form containing sine and cosine functions, while other joint angles are represented in inverse cosine form, thus establishing an explicit functional relationship between the joint angles and the arm angles.

[0104] Step S130: Extract the functional expressions of each joint angle with respect to the arm angle from the rotation matrix corresponding to the actual arm plane.

[0105] By matching the rotation matrix in the actual arm plane with the analytical form of the joint angles in the forward kinematics equations of the robotic arm, functional expressions for joints 1, 2, 3, 5, 6, and 7 with respect to the arm angle can be extracted. These functional expressions have explicit mathematical forms, including arctangent and arccosine functions, whose coefficients are determined by the configuration parameters of the robotic arm and the current end-effector pose. This accurately reflects the relationship between the joint angles and the arm angle, providing a mathematical model basis for subsequent joint constraint analysis and arm angle optimization.

[0106] By establishing a functional relationship between joint angles and arm angles, and solving and optimizing the arm angle range based on this relationship, it is possible to effectively prevent the joints of the robotic arm from exceeding the limits during movement, improve trajectory smoothness and motion reliability, and at the same time have good computational efficiency and practicality.

[0107] Specifically, step S300, which involves calculating the feasible arm angle range of the robotic arm during its movement based on the position information of the stationary point and the intersection point, includes:

[0108] Step S310: Based on the number of stationary points and the relative positional relationship between the intersection point and the stationary point, determine the distribution pattern of the function curve of each joint angle and arm angle relative to its corresponding angle limit line.

[0109] For each joint angle function with respect to the arm angle, the existence of zero, one, or two stationary points in the function curve is first determined based on the number of zeros of its derivative. Then, by solving the equation where the joint angle function value equals the upper or lower limit, the intersection point of the function curve and the joint limit line is obtained. Furthermore, by combining the properties of the stationary points (maximum or minimum points), the number of intersection points, and their relative distribution within the entire arm angle domain, the specific shape of the function curve is accurately determined, such as monotonically increasing, monotonically decreasing, or having one or more extreme points, providing a basis for subsequent interval division.

[0110] Step S320: Based on the distribution pattern, determine the arm angle sub-interval that makes the corresponding joint angle within its angle limit range.

[0111] Based on the curve shape identified in step S310, and combined with the calculated intersection values ​​with the upper and lower limit lines, the relationship between the function value and the joint limit value in different segments within the arm angle domain is analyzed. All arm angle interval segments that can satisfy the condition that the joint angle does not exceed the limit are determined by logical judgment. These interval segments may be continuous intervals or the union of multiple discontinuous intervals, and are ultimately uniformly defined as the feasible arm angle sub-intervals corresponding to the joint.

[0112] Step S330: Take the intersection of the arm angle sub-intervals corresponding to all joints to obtain the feasible arm angle intervals.

[0113] The intersection of the arm angle sub-intervals corresponding to all joints is taken to obtain the feasible arm angle intervals that simultaneously satisfy all joint limit constraints of the robotic arm in the current pose. The feasible arm angle sub-intervals corresponding to joints 1, 2, 3, 5, 6, and 7 obtained in step S320 are subjected to a set intersection operation; the result of the intersection operation is one or more continuous arm angle intervals, representing the range of arm angle values ​​that simultaneously ensure all related joints move within their upper and lower angle limits, providing a feasible selection range for finally selecting an optimal arm angle.

[0114] By systematically analyzing the functional relationship curves between the joint angles and the arm angles, and accurately calculating the feasible arm angle sub-intervals for each joint, the global feasible arm angle interval is obtained by finding the intersection. This effectively ensures that all joint angles calculated by inverse kinematics are strictly within the motion limits specified by the robotic arm body, thereby guaranteeing the feasibility, safety, and smoothness of trajectory execution and avoiding motion interruption or mechanism damage caused by joint over-limit.

[0115] Furthermore, the distribution patterns in step S320 include the following scenarios:

[0116] First, the function curve has two stationary points, with the maximum value below the upper limit of the joint angle and the minimum value above the lower limit. This first distribution pattern refers to the case where the function curve has two stationary points, with the maximum value below the upper limit of the joint angle and the minimum value above the lower limit. In this case, the overall fluctuation range of the function curve is entirely within the range defined by the upper and lower limits of the joint angle, and neither extreme value reaches the limit line. Therefore, the entire arm angle domain, i.e., the continuous interval from negative π to positive π, is a feasible arm angle sub-interval for this joint, and no interval truncation is required.

[0117] Second, the function curve has two stationary points, with the maximum value above the upper limit of the joint angle and the minimum value below the lower limit of the joint angle. The second distribution pattern refers to the case where the function curve has two stationary points, with the maximum value above the upper limit of the joint angle and the minimum value below the lower limit of the joint angle. In this case, the function curve exceeds both the upper and lower limits of the joint, and its intersection with the two joint limit lines will produce four intersection points. Based on the relative positional relationship between these intersection points and the two stationary points, logical judgment can be made to determine that the arm angle interval within the permissible range of the function value usually consists of three discontinuous intervals: from negative π to the first intersection with the upper limit, from the second intersection with the upper limit to the first intersection with the lower limit, and from the second intersection with the lower limit to positive π.

[0118] Third, the function curve has no stationary points. The third distribution pattern refers to the case where the function curve has no stationary points. This indicates that the functional relationship between the joint angle and the arm angle is monotonic; by solving the equation where the function value equals the upper and lower limits of the joint angle, at most two intersection points can be obtained; the feasible arm angle sub-interval of the joint is a continuous interval between these two intersection points, and whether the function is monotonically increasing or monotonically decreasing does not affect the way this continuous interval is determined.

[0119] Fourth, the function curve has a stationary point where the function value undergoes a step change. This fourth distribution pattern refers to the case where the function curve has a stationary point where the function value undergoes a step change. This situation is caused by a mathematical singularity, manifested as a jump of π radians in the function at the stationary point, thus dividing the entire arm angle domain into two monotonic intervals at the stationary point. The intersection points of the function and the joint limit line need to be solved in these two monotonic intervals respectively. Based on the limit values ​​on both sides of the jump point and the position of the intersection point, the two possible discontinuous intervals are comprehensively judged to form the feasible arm angle sub-intervals of the joint.

[0120] By systematically identifying and judging the distribution patterns of the four typical function curves mentioned above, and accurately calculating the feasible arm angle sub-intervals of each joint, a reliable foundation is provided for this method to finally obtain the global feasible arm angle interval by finding the intersection, ensuring that the inverse solution calculation results satisfy the constraint of all joints.

[0121] Furthermore, when the function curve has a stationary point and the function value undergoes a step change at the stationary point, step S320, based on the distribution pattern, determines the arm angle sub-interval that keeps the corresponding joint angle within its angular limit range, including:

[0122] Step S321: Calculate the left and right limits of the function at the stationary point.

[0123] Based on the functional expression of the joint angle with respect to the arm angle, it is obtained by taking the limits of the arm angle variable as it approaches the stationary point from the left and right sides, respectively. Since the function undergoes a step change at this point, its left and right limits are usually not equal, and the difference is approximately π radians. These two limit values ​​characterize the upper and lower boundaries of the function in the neighborhood of the stationary point, and are the key basis for judging the function behavior near this point and whether it satisfies the joint limit.

[0124] Step S322: When both the left and right limit values ​​are within the angular limit range of the corresponding joint, the stationary point is excluded from the arm angle interval.

[0125] Although the limit values ​​on both sides of the stationary point meet the requirements, the value of the function may be uncertain or exceed the limit because the function itself is undefined or undergoes a sudden change at the stationary point. Therefore, the point cannot be included in the feasible interval. In this case, the feasible arm corner interval consists of two consecutive intervals to the left and right of the stationary point, namely an open interval from negative π to the stationary point and another open interval from the stationary point to positive π.

[0126] Step S323: When the left limit value and / or the right limit value exceed the angle limit range, select the arm angle intervals on both sides of the stationary point that meet the joint angle limit requirements according to the intersection of the function relationship and the angle limit line, and include the selected arm angle intervals into the arm angle sub-intervals.

[0127] Specifically, within the monotonic intervals to the left and right of the stationary point, the equations with function values ​​equal to the upper and lower limits of the joint angle need to be solved to obtain the intersection point. Then, based on the specific situation of the limit value exceeding the limit (e.g., only the left side exceeds, only the right side exceeds, or both sides exceed) and the position of the intersection point, arm angle intervals with function values ​​continuously within the limit range are selected from the intervals on both sides, and these intervals are included in the feasible arm angle sub-intervals of the joint.

[0128] By performing the aforementioned specialized processing on function curves with step singularities, all feasible arm angle segments that satisfy joint constraint can be accurately and completely determined in such cases, ensuring the completeness and accuracy of subsequent interval intersection operations, thus providing a solid foundation for the safe and smooth motion planning of the robotic arm.

[0129] Furthermore, before establishing the functional relationship in step S100, the following steps are also included:

[0130] Step S101: Set global configuration parameters. Global configuration parameters are used to specify the target angle symbol combination for joints two, four, and six.

[0131] For multiple sets of inverse kinematic solutions existing under the same end pose, a target solution configuration can be uniquely determined by predefining the expected signs (positive or negative) of the three key joint angles. During the continuous movement of the robotic arm, this global configuration parameter is usually set according to the sign state of the joint angle solution used in the previous movement cycle, thereby ensuring that the configuration of the current solution is consistent with that of the previous moment in terms of sign, maintaining the continuity and consistency of the motion trajectory, and providing a prerequisite for establishing a definite functional relationship in the future.

[0132] Step S102: Based on the target angle symbol combination corresponding to the global configuration parameters, establish the functional relationship between each joint angle and the arm angle. The functional relationship corresponds to a set of joint angle solutions uniquely determined by the target angle symbol combination.

[0133] When constructing the functional relationship, the specified joint symbol combination is substituted as a known condition into the inverse kinematics derivation process based on arm angle parameterization. This ensures that all coefficients in the final functional expression of the joint angle with respect to the arm angle (including arctangent and arccosine functions) are associated with the symbol combination. This ensures that the established functional relationship only describes the solution branch under a specific symbol configuration, avoiding computational redundancy caused by solving all eight possible solutions.

[0134] By pre-setting global configuration parameters and establishing corresponding functional relationships based on them, the entire inverse kinematics calculation process always revolves around a set of defined solution branches that are continuous with the configuration of the previous time step. This not only significantly improves computational efficiency and avoids the huge computational overhead of traversing all solution branches, but more importantly, it ensures the consistency of the sign of the joint angle solutions during continuous trajectory tracking. This effectively prevents sudden changes in joint angles and severe shaking of the robotic arm caused by solution branch jumps, laying a stable and reliable foundation for subsequent joint limit checks and arm angle optimization.

[0135] Further, step S400, selecting the target arm angle based on the feasible arm angle range, includes:

[0136] Step S410: Based on the historical arm angle value of the previous control cycle and the upper and lower boundaries of the currently calculated feasible arm angle interval, the target arm angle value of the current control cycle is calculated through the bounded cosine function mapping relationship.

[0137] The actual arm angle value used at the previous moment is used as input and compared with the lower and upper limits of the current feasible arm angle interval. Based on the comparison result, an arm angle adjustment amount is calculated using a mathematical mapping relationship with bounded output values ​​constructed with a cosine function as the core. This adjustment amount will make the new arm angle value move closer to the center of the current feasible interval, but its adjustment range is constrained by the natural boundedness of the cosine function to avoid approaching or exceeding the boundary of the feasible interval due to over-adjustment. Among them, by adjusting the gain coefficient and scale factor in the function, the convergence speed of the arm angle approaching the center of the interval and the critical distance at which significant adjustment begins can be flexibly controlled, so as to smoothly generate a target arm angle value that satisfies all joint constraint constraints and is located as close as possible to the center of the feasible interval while ensuring computational efficiency. By adopting an adaptive arm angle selection scheme based on bounded cosine functions, the target arm angle at the current moment can be smoothly and controlledly generated within a known feasible arm angle interval, based on the arm angle state at the previous moment. This process is accurate and efficient, effectively avoiding jitter or abrupt changes in the arm angle near the interval boundary. At the same time, its inherent boundedness ensures that the output arm angle will never exceed the given feasible interval, thus providing a key guarantee for finally solving a set of safe, stable and continuous joint angle solutions.

[0138] Furthermore, the bounded cosine function mapping relationship is as follows:

[0139]

[0140] Where ψ(t) is the target arm angle value of the current control cycle, and ψ(t-1) is the historical arm angle value of the previous control cycle. all_upper and ψ all_lower Let be the upper and lower boundaries of the currently calculated feasible arm angle interval, respectively; K is a constant controlling the repulsion strength; and α is a constant controlling the initiation position of the repulsion reaction.

[0141] This mapping relationship uses historical arm angle values ​​as the starting point for calculation and compares them with the median of the current feasible interval. Its core construction utilizes the periodicity, smoothness, and boundedness of the cosine function to generate a limited adjustment amount. The magnitude of this adjustment amount depends on the relative position of the historical arm angle value within the feasible interval: when the historical value is close to the center of the interval, the adjustment amount approaches zero, keeping the arm angle stable; when the historical value deviates from the center and approaches any boundary, the adjustment amount increases significantly, pointing towards the center of the interval, thus driving the arm angle away from the boundary. The constant K is used to globally adjust the strength of this center-oriented behavior, while the constant α is used to control the critical position where the adjustment behavior begins to become significant, defining a "dead zone" range around the center of the interval. The output of the entire mapping relationship is strictly limited to a bounded range, ensuring that the final calculated target arm angle value will never exceed the given feasible arm angle interval.

[0142] Furthermore, the arm angle selection scheme based on bounded function mapping utilizes the mathematical properties of smooth continuous functions to adaptively generate a target arm angle value that is located within the interval and tends towards the central region, based on the arm angle state at the previous moment and the current feasible interval. This method not only has an efficient and well-defined calculation process, but more importantly, its output can absolutely guarantee that it will not exceed the safe area specified by the joint limit, while effectively avoiding frequent jittering or drastic changes in the arm angle value near the interval boundary.

[0143] The above control method will be explained in detail below using a specific example:

[0144] First, since each arm angle value corresponds to 8 valid solutions for the robotic arm, these 8 solutions can be distinguished based on the symbols of joints 2, 4, and 6. This is achieved by defining a symbol configuration parameter Gk. i (i = 2, 4, 6) is used to distinguish these 8 sets of solutions during the calculation process, and its specific definition is as follows:

[0145]

[0146] For an SRS-configured redundant robotic arm, its elbow E can be around the vector between the shoulder and elbow. Rotation creates an infinite number of solutions for a redundant robotic arm. For example... Figure 1 As shown, plane WE v S is the reference arm plane, WES is the arm plane in which the actual configuration is located, and ψ is the arm angle. Figure 2 This is the virtual arm planar computation diagram, where xyz is the base coordinate system.

[0147] The position vector of the shoulder S relative to the base coordinate system can be given by the following formula:

[0148]

[0149] When performing inverse kinematics, given the end pose Then, the wrist position can be calculated using the following formula:

[0150]

[0151] Then, the vector p can be calculated using the following formula. 26 Right now

[0152]

[0153] Due to vectors with vector The module length is the fixed parameters d3 and d5 of the robotic arm link, so the virtual joint... It can be calculated using the Law of Cosines:

[0154]

[0155] Similarly, Figure 2 φ in the equation can also be calculated using the Law of Cosines:

[0156]

[0157] Next, using the alternate interior angle theorem, the virtual joint is obtained. It can be obtained from the following formula, where p26(x), p26(y), and p26(z) are vectors p 26 Components on the three axes of the base coordinate system:

[0158]

[0159] Reference arm plane WE v There are many ways to define S, but the most intuitive way is to use the arm plane corresponding to the vertically upward configuration of the robotic arm elbow as the reference arm plane. The rotation of joint 3 directly affects the shape of the robotic arm elbow, so position 0 is used as a virtual joint. Configuration.

[0160] because Therefore, vector p 26 The angle formed by the projection vector onto the xy plane of the base coordinate system and the x-axis component of the base coordinate system is the virtual joint 1. It can be given by the following formula.

[0161]

[0162] At this point, the virtual joints are located in the virtual arm plane. All have been calculated. At this point, Substituting into the forward kinematics equations, we obtain the forward kinematics equations corresponding to the virtual arm plane. Extract the rotation matrix from it.

[0163] Depend on Figure 1 It can be seen that the actual arm plane WES is derived from the reference arm plane WE. v S around the vector Obtained by rotating the arm angle ψ. Rotation matrix. Each vector consists of three sets of column vectors. Considering that the Rodrigues rotation transformation can calculate the vector after rotating a given vector around a fixed axis, the Rodrigues rotation transformation is used for... The transformation can be used to obtain Around the vector The actual arm plane corresponding to the arm angle ψ after rotation This process is given by the following formula:

[0164]

[0165] Further simplification of the above formula yields:

[0166]

[0167] Once known, the corresponding θ1, θ2, and θ3 of the actual arm plane can be solved algebraically using the following formula.

[0168]

[0169] in,

[0170]

[0171] Depend on available It is obtained from the following formula,

[0172]

[0173] Further simplification of the above formula yields:

[0174]

[0175] Once known, the corresponding θ5, θ6, and θ7 of the actual arm plane can be solved algebraically using the following formula.

[0176]

[0177] From the above simplified formula:

[0178]

[0179] We can obtain:

[0180]

[0181] Substituting the above equation back into the solution for θ1, θ2, θ3, θ5, θ6, θ7, we obtain the following function relating the angles of joints 1, 3, 5, 7 and joints 2, 6 to the arm angle ψ.

[0182]

[0183] Based on the functional properties of arcos and atan2, the functional equation has only discontinuities of the first kind in its domain, meaning these discontinuities and jump discontinuities can be eliminated. Furthermore, the function is smooth and continuous outside these discontinuities. This provides a possibility for determining the range of arm angles that allows the joint to remain within its limits.

[0184] Next, we differentiate the above two equations with respect to the arm angle ψ using the chain rule, and finally obtain the following equation. Only the formulas for joints 1, 2, and 3 are given here; the formulas for joints 5, 6, and 7 are derived similarly.

[0185]

[0186]

[0187] When dealing with joints 1 and 3, let θ i The stationary point of the function is obtained by ψ′ = 0, as shown in the following equation. The formulas corresponding to joints 5 and 7 are derived similarly.

[0188]

[0189] K at =Gk s (K cn K bd -K bn K cd );

[0190] K bt =Gk s (K an K cd -K cn K ad );

[0191] K ct =Gk s (K an K bd -K bn K ad ).

[0192] Observing the above formula, we can find that the number and properties of the stationary points of this function can be determined by the formula under the square root K. at 2 +K bt 2 -K ct 2 Let's analyze

[0193] When K at 2 +K bt 2 -K ct 2 >0: This holds true, meaning the function has two stationary points.

[0194] When K at 2 +K bt 2 -K ct 2 <0 o'clock: This is not true, meaning the function does not have a stationary point.

[0195] When K at 2 +K bt 2 -K ct 2 =0: This holds true if the function has only one stationary point, which is expressed as follows:

[0196]

[0197] Substituting the above equation into θ i (ψ) indicates that:

[0198] Gk s [K an sin(ψ)+K bn cos(ψ)+K cn ] = 0;

[0199] Gk s [K ad sin(ψ)+K bd cos(ψ)+K cd ] = 0.

[0200] In this case, θ cannot be determined. i The magnitude of θ leads to algorithmic singularities. However, the curve's shape can still be determined by the limit analysis function's limit within the neighborhood of the stationary point ψ0. Substituting ψ0+σ back into θ... i (ψ) gives the following formula

[0201]

[0202] Taking σ as a variable, we obtain the left and right limits respectively as follows:

[0203]

[0204] It can be observed that the joint angles on both sides of ψ0 differ by π radians, but the function curve still follows the principle of having only one stationary point. Although θ i (ψ) has a difference of π radians on both sides of ψ0, but the intervals [-π,ψ0) and (ψ0,π) on both sides of ψ0 have no stationary points (i.e., it is a monotonic function).

[0205] Through experiments, when K at 2 +K bt 2 -K ct 2 When the value is greater than 0 (i.e., when there are 2 stationary points), the function curve is as follows: Figure 4a , Figure 4b , Figure 4c and Figure 4d As shown.

[0206] When K at 2 +K bt 2 -K ct 2 When <0 (i.e., when there is no stationary point), the function curve is as follows: Figure 5a and Figure 5b As shown.

[0207] When K at 2 +K bt 2 -K ct 2 When = 0 (i.e., there is only one stationary point, and a singularity occurs), the function curve is as follows: Figure 6a and Figure 6b As shown.

[0208] When dealing with joint 2, let θ i The stationary point of the function is obtained by ψ′=0, as shown in the following equation. The formula corresponding to joint 6 is derived similarly.

[0209]

[0210] Observing the above formula, we can find that the number and properties of the stationary points of this function can be determined by the formula under the square root K. a 2 +K b 2 Let's analyze

[0211] When K a 2 +K b 2 When >0, This holds true, meaning the function has two stationary points.

[0212] When K a 2 +K b 2 When = 0, This holds true if the function has only one stationary point.

[0213] Experiments have shown that:

[0214] When K a 2 +K b 2 When there are two stationary points at >0, the function curve is as follows: Figure 7a , Figure 7b and Figure 7c As shown; when K a 2 +K b 2 When K = 0, there is only one stagnation point with a singularity, i.e., K. a =0,K b =0, making

[0215] const = θ i (ψ)=arcos(Gk s K c );

[0216] It can be observed that θ at this time i (ψ) is a constant const, so we can directly determine the feasible arm angle interval by whether the constant const exceeds the joint limit. If const exceeds the joint limit, there is no feasible arm angle interval. If the joint limit is satisfied, the feasible arm angle interval is the entire range [-π,π].

[0217] The function curve at this time is as follows Figure 8 As shown, it is indeed a constant.

[0218] Below, we analyze the methods for determining the feasible arm angle range under different joint constraints based on the various curve configurations described above.

[0219] In the following analysis, dots represent the intersections of joint limit lines and function curves, and squares represent stationary points of the function curve. When analyzing a function curve with only one stationary point, the stationary point is represented by ψ0; when analyzing a function curve with two stationary points, the left stationary point is represented by ψ0. 0_left The right stationary point is represented by ψ. 0_right The points where the joint limit line intersects the function curve, starting from the leftmost point of the arm angle and moving to the right, are sequentially labeled ψ with the points where they intersect the upper limit line of the joint. intermaxi (i = 1, 2, 3...), the intersections with the lower limit line of the joint are numbered sequentially as ψ. intermini (i = 1, 2, 3...). The upper limit value of the joint is labeled qmax, and the lower limit value is labeled qmin.

[0220] The analysis process requires obtaining the function θ. i (ψ) = qmax or θ i (ψ)=qmin corresponds to ψ intermini (i = 1, 2, 3…) and ψ intermaxi(i = 1, 2, 3…), so θ is set in advance. i The inverse function of (ψ) ψ(θ) i The following is given.

[0221] Regarding ψ(θ) of joints 1 and 3 i ) is: (ψ(θ) of joints 5 and 7 i (Similar derivation)

[0222]

[0223] K ap =Gk s ((K cd -K bd )tan(θ i )+(K bn -K cn ));

[0224] K bp =2Gk s (K ad tan(θ i )-K an );

[0225] K cp =Gk s ((K cd +K bd )tan(θ i )-(K bn +K cn ));

[0226] Based on the above formula, we get:

[0227] If K bp 2 -4K ap K cp =0, then ψ(θ) i There is only one possible value, corresponding to one intersection point.

[0228] If K bp 2 -4K ap K cp >0, then ψ(θ) i There are two possible values, corresponding to two intersection points.

[0229] If K bp 2 -4K ap K cp <0, then ψ(θ) i Are there any possible values ​​that correspond to no intersection points?

[0230] Regarding ψ(θ) of joint 2i ) is: (ψ(θ of joint 6 i ) Similar derivation)

[0231]

[0232] According to the above formula, we get:

[0233] If K a 2 +K b 2 -(K c -cos(θ i )) 2 =0, then ψ(θ i ) has only one possible value, corresponding to one intersection point.

[0234] If K a 2 +K b 2 -(K c -cos(θ i )) 2 >0, then ψ(θ i ) has two possible values, corresponding to two intersection points.

[0235] If K a 2 +K b 2 -(K c -cos(θ i )) 2 <0, then ψ(θ i ) has no possible value, corresponding to no intersection point.

[0236] Then, analyze the curve profiles of joints 1, 3, 5, and 7.

[0237] Case of two stationary points:

[0238] For Figure 4a : When Figure 9a occurs, that is, when there is no intersection between the upper and lower limits of the joint and the function curve, the feasible arm angle interval is [-π, π].

[0239] When Figure 9b occurs, θ i (ψ 0_right ) < qmin and θ i (ψ 0_left ) > qmax, the feasible arm angle interval is [-π, ψ intermax1 ∪ [ψ intermax2 , ψ intermin1 ∪ [ψ intermin2[π]. Other reverse symmetric cases are similar.

[0240] When it happens Figure 9c In the case of θ i (ψ 0_right )≥qmin and θ i (ψ 0_left If qmax > 0, the feasible arm angle interval is [-π, ψ]. intermax1 ]∪[ψ intermax2 [π]. Other reverse symmetric cases are similar.

[0241] against Figure 4b When it happens Figure 4b In cases where there is an intersection with the upper or lower limit of the joint, it may be related to... Figure 4a When a conflict arises in the judgment logic, it is necessary to calculate the two stationary points ψ. 0_right and ψ 0_left We can use the first derivative values ​​on both sides of the value to determine whether it is a maximum or minimum point. Figure 4b The first discontinuity of kind occurs in θ i (ψ) lies on the boundary of -π or π. In this case, the midpoints of -π or π and the two extreme points can be calculated respectively. The case in Figure 10 is the calculation of... and (The derivation for other reverse symmetric cases is similar). In this case, the joint limit line passing through the midpoint will definitely intersect the curve at two points, and the intersection point ψ between the corresponding midpoint and the function curve is calculated. intermax1 ,ψ intermax2 ,ψ intermin1 ,ψ intermin2 If ψ intermax1 ,ψ intermax2 With ψ intermin1 ,ψ intermin2 All are distributed at the corresponding ψ 0_right With ψ 0_left If both sides show a maximum value less than a minimum value, then it is determined that the condition has entered a certain range. Figure 4b The following analysis will be conducted on this curved profile.

[0242] When it happens Figure 10a In the case where the upper and lower limits of the joint do not intersect with the function curve, the feasible arm angle interval is [-π, π].

[0243] When it happens Figure 10b In the case of θ, calculate i (-π) and θ i The boundary value of (π). If θ i (-π) and θ i If (π) is not within [qmin, qmax], then the feasible arm angle interval is [ψ]. intermax1 ,ψ intermax2 ]∪[ψintermin1 ,ψ intermin2 ].

[0244] When it happens Figure 10c In the case of θ i (-π) is within [qmin, qmax] and θ i If (π) is not within [qmin, qmax], then the feasible arm angle interval is [-π, ψ(π))∪[ψ intermin1 ,ψ intermin2 Other reverse-symmetric cases are similar.

[0245] against Figure 4c :when Figure 4c When this happens, it will be with Figure 4a and Figure 4b Conflicts arise, necessitating a method to determine this curve type, and this method is related to analysis. Figure 4b The curve outline is similar. Figure 4c The first type of discontinuity in θ also occurs in θ. i (ψ) lies on the boundary of -π or π. In this case, the midpoints of -π or π and the two extreme points can be calculated respectively. The case in Figure 11 is the calculation of... and (The derivation for other reverse symmetric cases is similar). In this case, the joint limit line passing through the midpoint will definitely intersect the curve at two points, and the intersection point ψ between the corresponding midpoint and the function curve is calculated. intermax1 ,ψ intermax2 ,ψ intermin1 ,ψ intermin2 If ψ intermax1 ,ψ intermax2 With ψ intermin1 ,ψ intermin2 There exists a distribution not corresponding to ψ 0_right With ψ 0_left If one of the two sets of values ​​is a maximum value smaller than the minimum value, then it is determined that the condition has entered a certain range. Figure 4c The following analysis will be conducted on this curved profile.

[0246] When it happens Figure 11a In the case where the upper limit of the joint does not intersect with the function curve, the feasible arm angle interval is [-∏,π].

[0247] When it happens Figure 1 In case 1b, calculate ψ intermin1 ,ψ intermin2 ,ψ intermax1 ,ψ intermax2 and ψ 0_left ,ψ 0_right .

[0248] If ψ 0_left Belongs to [ψ]intermin1 ,ψ intermin2 ], then ψ intermin1 ,ψ intermin2 ,ψ 0_left They were grouped together. Then, because... With ψ 0_right -ψ 0_left Since the signs are the same, ψ intermax2 ,ψ 0_right Grouped together, ψ intermax1 They are grouped separately. At this time, Figure 11b The feasible arm angle interval under the given condition is [-∏, ψ] intermax1 ]∪[ψ intermin1 ,ψ intermin2 ]∪[ψ intermax2 ,∏]. Other cases of reverse symmetry are similar.

[0249] When it happens Figure 11c In cases where the lower limit of the joint does not intersect with the curve function, continue with the analysis. Figure 11b The way to ψ intermax2 ,ψ 0_right Grouped together, ψ intermax1 This is a separate group. The feasible arm angle interval is [-∏, ψ]. intermax1 ]∪[ψ intermax2 ,∏]. Other reverse symmetric cases are similar.

[0250] When it happens Figure 11d In this case, the upper limit of the joint and the curve function have no intersection point, and the feasible arm angle interval is [ψ]. intermin1 ,ψ intermin2 Other reverse-symmetric cases are similar.

[0251] against Figure 4d :when Figure 4d When this happens, ψ will appear. 0_right =±π or ψ 0_left The case where =±π.

[0252] when Figure 12a When this situation occurs, the upper and lower limits of the joint do not intersect with the function curve. The feasible arm angle interval is [-π, π].

[0253] when Figure 12b When the situation occurs, the lower limit of the joint and the curve function have no intersection and ψ 0_right =π,θ i (ψ 0_right )≥qmin,θ i (ψ 0_left If qmax > 0, the upper limit of the joint intersects the function curve at two points. The feasible arm angle interval is [-π, ψ]. intermax1 ]∪[ψ intermax2,[π]. Other cases of reverse symmetry are similar.

[0254] when Figure 12c When the situation occurs, θ i (ψ 0_right )≤qmin and θ i (ψ 0_left The feasible arm angle interval is [ψ] ≤ qmax. intermin1 ,ψ intermin2 Other reverse-symmetric cases are similar.

[0255] when Figure 12d When the situation occurs, θ i (ψ 0_right )≤qmin and θ i (ψ 0_left The feasible arm angle interval is [ψ > qmax]. intermin1 ,ψ intermax1 ]∪[ψ intermax2 ,ψ intermin2 Other reverse-symmetric cases are similar.

[0256] against Figure 5a When a function curve has no stationary points, it is a basic monotonic function, such as... Figure 13 As shown.

[0257] for Figure 13 In this case, the feasible arm angle interval can be directly given as [ψ]. intermin1 ,ψ intermax1 Other reverse-symmetric cases are similar.

[0258] The case where there is a stationary point (singular case):

[0259] against Figure 5b When it happens Figure 14a In this case, the upper and lower limits of the joint do not intersect the function curve. The feasible arm angle interval is [-π, ψ0) ∪ (ψ0, π). Other reverse symmetric cases are similar.

[0260] When it happens Figure 14b In this case, the feasible arm angle interval [-π, ψ] can also be directly obtained from the intersection points of the function curve and the upper and lower limits of the joint. intermin1 ]∪[ψ intermax1 [π]. Other reverse symmetric cases are similar.

[0261] When it happens Figure 14c In the case where the lower limit of the joint does not intersect the function curve, but the upper limit of the joint does intersect the function curve, the feasible arm angle interval is obtained as [-π, ψ0) ∪ [ψ0]. intermax1 [π]. Other reverse symmetric cases are similar.

[0262] The above is an analysis of the function curves and joint limits for joints 1, 3, 5, and 7. The function curves for joints 2 and 6 are similar to... Figure 4a Similarly, we will not go into detailed derivation here.

[0263] After the above analysis, a feasible range of arm angles was obtained that satisfies the condition that joints 1, 3, 5, 7, 2, and 6 do not exceed the joint limits, denoted as [missing information].

[0264] Next, seek denoted as ψ all ψ all That is, the total intersection ψ of the feasible arm angle intervals that satisfy the condition that the corresponding joints 1, 3, 5, 7, 2, and 6 do not exceed the joint limit. all =[ψ all_lower ,ψ all_upper ].

[0265] Finally, we provide an arm angle selection function based on a bounded cosine function that aims to position the arm angle at the center of the total intersection of feasible arm angle intervals, within the total intersection ψ. all Select a suitable arm angle and solve the inverse kinematics.

[0266] Where K is a constant that controls the arm angle relative to the total intersection boundary ψ. all_lower ,ψ all_upper The repulsion strength is α, which is a constant and controls the limiting distance at which the arm angle begins to repel the reaction.

[0267]

[0268] This invention is mainly used in the inverse kinematics process of a robotic arm's end effector performing Cartesian motion. The general application steps are as follows:

[0269] In a Cartesian motion sequence, the joint angle θ at the current moment is first obtained. ref And obtain θ through forward kinematics ref The corresponding homogeneous transformation matrix of the robotic arm end effector Then, based on the trajectory planning results, the homogeneous transformation matrix of the target's end pose at the next moment is obtained. The arm angle ψ(t-1) selected in the previous solution process is known, and the end-effector pose can be obtained through arm angle analysis. Time joint angle θ i The functional relationship between the arm angle ψ and the joint is determined. After obtaining the functional relationship, the curve profile corresponding to each joint is analyzed. Then, the intersection of the curve profile with the upper and lower limit lines of each joint is analyzed to obtain the feasible arm angle interval that ensures each joint does not exceed the joint limit. Finally, the intersection of the feasible arm angle intervals of each joint is calculated to obtain the total feasible arm angle interval ψ. allThen, the optimal arm angle ψ(t) that satisfies the joint limit is obtained by substituting it into the adaptive arm angle generating function based on the cosine function, and the joint angle solution at the current time is obtained by kinematic solution using the arm angle method.

[0270] Step S1: Obtain the joint angles θ at time t-1 t-1 And the corresponding homogeneous transformation matrix is ​​obtained through forward kinematics.

[0271] Step S2: Obtain the target end pose at time t The arm angle ψ(t-1) at time t-1 is known.

[0272] Step S3: Through The joint angles corresponding to the end-effector pose in the aforementioned reference arm plane can be obtained. This leads to the obtaining of the reference arm plane. And obtain the reference arm plane around vector The actual arm plane corresponding to the arm angle ψ after rotation and Then, after performing elimination operations on the following forward kinematic equations, establish an equation relationship with the first two equations.

[0273]

[0274]

[0275] This leads to the joint angle θ. i Functional relationship between arm angle ψ and:

[0276]

[0277] Step S4:

[0278] The above formulas are obtained by differentiating them with respect to the arm angle ψ (the derivation for joints 5, 7, and 6 is similar).

[0279]

[0280] By making the derivative expression equal to 0, we obtain the formula for finding the stationary points of the curve contour at points 1 and 3 (the derivation for joints 5 and 7 is similar).

[0281]

[0282] K at =Gk s (K cn K bd -K bn K cd );

[0283] Kbt =Gk s (K an K cd -K cn K ad );

[0284] K ct =Gk s (K an K bd -K bn K ad ).

[0285] Formula for determining the stationary point of the curve profile of joint 2 (similar derivation for joint 6).

[0286]

[0287] Step S5: After obtaining the formula for determining stationary points, curve profile analysis can be performed. First, the state and shape of the curve profile are determined by the number of stationary points. The number of stationary points is determined by the discriminant of the following roots.

[0288] When K at 2 +K bt 2 -K ct 2 When >0: This means the function has two stationary points.

[0289] When K at 2 +K bt 2 -K ct 2 When <0: This means the function does not have any stationary points.

[0290] When K at 2 +K bt 2 -K ct 2 =0: This means that the function has only one stationary point.

[0291] When K a 2 +K b 2 When the value is greater than 0, the function has two stationary points.

[0292] When K a 2 +K b 2 When = 0, the function has only one stationary point. Then, based on the relationship between the function curve value at the stationary point and the corresponding upper and lower limit values ​​of the joint, the arm angle interval that allows the corresponding joint to be within the upper and lower limit values ​​is determined. Finally, the intersection of these feasible arm angle intervals is calculated to obtain the total feasible arm angle interval ψ that simultaneously ensures that joints 1, 2, 3, 5, 6, and 7 are within the upper and lower limits. all

[0293] Step S6: Obtain the interval boundary value ψ of the total feasible arm angle interval. all_lower ,ψ all_upper The arm angle ψ(t-1) selected and used in the solution at time t-1, along with the edge values ​​of the two intervals, is substituted into the adaptive arm angle selection function based on the cosine function to select the arm angle ψ(t) at time t.

[0294]

[0295] Step S7: Substitute ψ(t) into the following functional equation to obtain the joint angle value corresponding to the arm angle ψ(t) and use it as the value at time t. The inverse solution is then sent to the robotic arm for execution. Since the arm angle is selected within the total feasible arm angle range, it is guaranteed that the calculated joint angle value will be within the joint limit range.

[0296]

[0297] Next, a kinematic model of a seven-axis robotic arm is used to conduct motion experiments in Cartesian space to verify whether the proposed method can avoid joint limits. Its DH parameters and joint limit parameters are shown in the table below.

[0298]

[0299]

[0300] The experimental part of this invention sets a set of relatively extreme initial configurations for the robotic arm [160°, -20°, 50°, 110°, -40°, 120°, 160°], and the initial pose transformation matrix is:

[0301]

[0302] Observing this initial configuration, we can see that joints 1, 6, and 7 are all close to their joint limits. Now, with the robot arm's end effector set to maintain its orientation, its position is moved 0.15m along both the x and y axes. Based on the solution from the previous time step, determine the global configuration parameter Gk for the current time step. i And obtain a set of inverse solutions after arm angle optimization. During the verification process, a general fixed arm angle solution selection method is incorporated for comparative experiments. Joint trajectory comparison, for example... Figure 15a and Figure 15b As shown.

[0303] It can be seen from the fixed arm angle method ( Figure 15aThe joint trajectory obtained has become singular due to joint 7 exceeding the limit, resulting in abrupt changes in the joint trajectory. The joint trajectory obtained by the arm angle optimization method ( Figure 15b In the model, the trajectories of joints 1, 6, and 7 all have a limiting trend, thus making the entire joint trajectory continuous and smooth.

[0304] Accordingly, please refer to Figure 16 A second aspect of this invention provides an SRS configuration robotic arm inverse motion control system, which controls the SRS configuration robotic arm based on the above-described SRS configuration robotic arm inverse motion control method. The control system includes:

[0305] The stationary point acquisition module 1 is used to establish the functional relationship between several joint angles of the seven-degree-of-freedom robotic arm and the arm angle, and to determine the stationary point of each functional relationship. The arm angle is defined as the rotation angle of the actual arm plane relative to the reference arm plane around the line connecting the shoulder to the wrist.

[0306] Intersection point acquisition module 2 is used to acquire the intersection point of each functional relationship with the preset angle limit line of the corresponding joint;

[0307] The arm angle calculation module 3 is used to calculate the feasible arm angle range of the robotic arm during the movement process based on the position information of the stationary point and the intersection point. The feasible arm angle range is the range of arm angle values ​​that ensures that all joint angles are within their corresponding angle limits.

[0308] The instruction generation module 4 is used to select the target arm angle according to the feasible arm angle range and generate joint angle control instructions for the robotic arm based on the target arm angle.

[0309] Accordingly, a third aspect of the present invention provides an electronic device, including: at least one processor; and a memory connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to cause the at least one processor to perform the above-described SRS configuration robotic arm inverse motion control method.

[0310] Accordingly, a fourth aspect of the present invention provides a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the above-described SRS configuration robotic arm inverse motion control method.

[0311] The embodiments of this invention aim to protect a method and system for reverse motion control of an SRS-configured robotic arm, which has the following effects:

[0312] 1. By systematically constructing the functional relationship between joint angles and arm angles and analyzing its curve characteristics (such as stationary points and intersections with limit lines), the feasible arm angle range in which all joints do not exceed the limits can be accurately calculated. This fundamentally solves the problem of joints easily exceeding limits and trajectory changes in the Cartesian space motion of a seven-degree-of-freedom robotic arm, ensuring the reliability and safety of the motion.

[0313] 2. By introducing global configuration parameters to pre-lock the desired robotic arm configuration, this invention avoids the huge computational overhead of traditional methods that require traversing all eight sets of inverse solutions before screening, and also overcomes the computational redundancy and local convergence risk brought about by the random search of intelligent optimization algorithms, making online real-time calculation efficient and the results deterministic.

[0314] 3. The adaptive arm angle selection strategy based on the bounded cosine function can smoothly generate the optimal arm angle at the current moment within the feasible arm angle range based on the arm angle value at the previous moment. Its output value is naturally bounded, thus effectively avoiding arm angle jumps and sudden changes in joint velocity, and finally generating a continuous, smooth and natural joint trajectory.

[0315] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0316] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0317] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0318] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0319] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the specific implementation of the present invention. Any modifications or equivalent substitutions that do 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 inverse motion control of an SRS-configured robotic arm, characterized in that, Includes the following steps: Establish the functional relationship between several joint angles of a seven-degree-of-freedom robotic arm and the arm angle, and determine the stationary point of each of the aforementioned functional relationships. The arm angle is defined as the rotation angle of the actual arm plane relative to the reference arm plane around the line connecting the shoulder and wrist. Obtain the intersection point of each of the aforementioned functional relationships and the preset angle limit line of the corresponding joint; Based on the position information of the stationary point and the intersection point, the feasible arm angle range during the movement of the robotic arm is calculated. The feasible arm angle range is the range of arm angle values ​​that ensures that all joint angles are within their corresponding angle limits. Select a target arm angle based on the feasible arm angle range, and generate joint angle control commands for the robotic arm based on the target arm angle; The step of selecting the target arm angle based on the feasible arm angle range includes: Based on the historical arm angle value of the previous control cycle and the upper and lower boundaries of the currently calculated feasible arm angle interval, the target arm angle value of the current control cycle is calculated through the bounded cosine function mapping relationship. The bounded cosine function mapping relationship is as follows: in, The target arm angle value for the current control cycle. This is the historical arm angle value from the previous control cycle. and Let be the upper and lower boundaries of the currently calculated feasible arm angle interval, respectively. To control the constant of repulsion strength, A constant used to control the initiation site of the repulsion reaction; The establishment of the functional relationships between the joint angles of the seven-degree-of-freedom robotic arm and the arm angle includes: Based on the configuration parameters of the robotic arm, the arm plane with the elbow pointing vertically upward when the arm angle is zero degrees is determined as the reference arm plane, and the virtual joint angle corresponding to the reference arm plane is calculated. The rotation matrix corresponding to the actual arm plane obtained by rotating the reference arm plane around the line connecting the shoulder and wrist by the arm angle is calculated using the Rodriguez rotation transformation formula. From the rotation matrix corresponding to the actual arm plane, the functional expressions of each joint angle with respect to the arm angle are extracted; Among them, the function expression of some joint angles is in arctangent form, which includes sine and cosine functions, while the function expression of other joint angles is in arccosine form.

2. The inverse motion control method for the SRS configuration robotic arm according to claim 1, characterized in that, The step of calculating the feasible arm angle range of the robotic arm during its movement based on the position information of the stationary point and the intersection point includes: Based on the number of the stationary points and the relative positional relationship between the intersection point and the stationary point, determine the distribution pattern of the function curve of each joint angle and arm angle relative to its corresponding angle limit line; Based on the distribution pattern, determine the arm angle sub-interval that makes the corresponding joint angle within its angle limit range; The feasible arm angle interval is obtained by taking the intersection of the arm angle sub-intervals corresponding to all joints.

3. The inverse motion control method for the SRS configuration robotic arm according to claim 2, characterized in that, The distribution patterns include: The function curve has two stationary points, and the maximum point is lower than the upper limit of the joint angle and the minimum point is higher than the lower limit of the joint angle. The function curve has two stationary points, with the maximum point being higher than the upper limit of the joint angle and the minimum point being lower than the lower limit of the joint angle. The function curve does not have any stationary points; The function curve has a stationary point, and the function value undergoes a step change at the stationary point.

4. The inverse motion control method for the SRS configuration robotic arm according to claim 3, characterized in that, When the function curve has a stationary point and the function value undergoes a step change at the stationary point, determining the arm angle sub-interval that keeps the corresponding joint angle within its angle limit range based on the distribution pattern includes: Calculate the left and right limits of the function at the stationary point; When both the left limit value and the right limit value are within the angle limit range of the corresponding joint, the stationary point is excluded from the arm angle sub-range. When the left limit value and / or the right limit value exceed the angle limit range, the arm angle intervals that meet the joint angle limit requirements on both sides of the stationary point are selected according to the intersection of the function relationship and the angle limit line, and the selected arm angle intervals are included in the arm angle sub-intervals.

5. The inverse motion control method for the SRS configuration robotic arm according to claim 1, characterized in that, Before the step of establishing the functional relationship between the joint angles of the seven-degree-of-freedom robotic arm and the arm angle, the following steps are also included: Set global configuration parameters, which are used to specify the target angle symbol combination for joints two, four, and six; Based on the target angle symbol combination corresponding to the global configuration parameters, a functional relationship between each joint angle and arm angle is established, and the functional relationship corresponds to a set of joint angle solutions uniquely determined by the target angle symbol combination.

6. A reverse motion control system for an SRS-configured robotic arm, characterized in that, The SRS configuration robotic arm is controlled based on the inverse motion control method of any one of claims 1-5, and the control system includes: The stationary point acquisition module is used to establish the functional relationship between several joint angles of the seven-degree-of-freedom robotic arm and the arm angle, and to determine the stationary point of each functional relationship. The arm angle is defined as the rotation angle of the actual arm plane relative to the reference arm plane around the line connecting the shoulder and wrist. The intersection point acquisition module is used to acquire the intersection point of each of the aforementioned functional relationships and the preset angle limit line of the corresponding joint; The arm angle calculation module is used to calculate the feasible arm angle range of the robotic arm during the movement process based on the position information of the stationary point and the intersection point. The feasible arm angle range is the range of arm angle values ​​that makes all joint angles within their corresponding angle limits. The instruction generation module is used to select a target arm angle based on the feasible arm angle range, and generate joint angle control instructions for the robotic arm based on the target arm angle.

7. An electronic device, characterized in that, include: At least one processor; And a memory connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to cause the at least one processor to perform the SRS configuration robotic arm inverse motion control method as described in any one of claims 1-5.

Images