SRS configuration mechanical arm inverse motion control method and system

Abstract

The invention discloses an inverse motion control method and system for an SRS configuration mechanical arm, and the method comprises the steps: building a function relation between each of a plurality of joint angles of a seven-degree-of-freedom mechanical arm and an arm angle, determining a stagnation point of each function relation, and defining the arm angle as a rotation angle of an actual arm plane around a connecting line from a shoulder to a wrist relative to a reference arm plane; the intersection point of each function relation and the preset angle limiting line of the corresponding joint is obtained; according to the position information of the stationary point and the intersection point, the feasible arm angle interval of the mechanical arm in the moving process is calculated, and the feasible arm angle interval is the arm angle value range enabling all joint angles to be within the corresponding angle limiting range; and a target arm angle is selected according to the feasible arm angle interval, and a joint angle control instruction of the mechanical arm is generated based on the target arm angle. Through systematic analysis of the function relation between the joint angle and the arm angle, the feasible arm angle interval is accurately calculated, and efficient and reliable online avoidance of the joint movement limit of the seven-degree-of-freedom mechanical arm is achieved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm control, and particularly relates to a SRS configuration mechanical arm inverse motion control method and system. BACKGROUND

[0002] Seven-degree-of-freedom (7-DOF) manipulators exhibit superior motion flexibility in complex and unstructured environments due to their anthropomorphic configuration. Compared with traditional 6-DOF manipulators, the additional seventh degree of freedom introduces redundancy, which enables the manipulator to perform self-motion in its null space while keeping the end-effector pose unchanged. This property provides the possibility to optimize the motion performance of the manipulator, such as joint limit avoidance, obstacle avoidance, and energy consumption optimization, but also greatly increases the complexity of solving the inverse kinematics problem.

[0003] To solve the inverse kinematics problem of 7-DOF manipulators, a common analytical method is to set a redundant parameter to reduce the 7-DOF problem to a 6-DOF problem. For a bias-free SRS (shoulder-elbow-wrist) configuration manipulator, using the arm angle as the redundant parameter is a widely used strategy. The arm angle parameter has good geometric intuition and can clearly describe the rotation motion of the elbow around the shoulder-wrist connecting line, making it easy to directly intervene and optimize the configuration of the manipulator.

[0004] After obtaining multiple sets of solutions of inverse kinematics through the arm angle parameter, how to select an optimal solution from them becomes the key to determining the motion performance of the manipulator. Existing solution selection strategies mainly focus on the continuity of joint motion and the avoidance of joint limits. The most intuitive method is to traverse 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 search for solutions that meet the optimization objectives in the solution space. The introduction of the arm angle concept makes it possible to directly intervene in the arm angle according to the optimization objectives to select the solution.

[0005] However, these existing technical solutions have obvious limitations. The traversal method has low computational efficiency, and the calculation time increases significantly with the improvement of solving accuracy. In addition, the selection of discrete step length is difficult to grasp, which may lead to the generation of joint trajectory with jagged phenomenon. Although the intelligent optimization algorithm is easy to add complex optimization indicators, it is essentially dependent on random search, which has the risk of large calculation overhead, slow convergence speed and possible local optimal solution. More importantly, the current optimization strategy based on arm angle is not comprehensive in analyzing the function relationship curve between joint angle and arm angle, and it fails to systematically reveal the internal relationship between different curve shapes (such as the number of stationary points, singular points) and joint limits. In addition, there is no complete method for solving the feasible arm angle interval for various possible curve shapes. This leads to a lack of an effective solution that can efficiently and reliably select the appropriate arm angle to strictly avoid the joint motion limit when the end of the robot arm performs Cartesian space motion. SUMMARY

[0006] The embodiment of the present application aims to provide a SRS configuration robot arm inverse motion control method and system, which accurately calculates the feasible arm angle interval through systematic analysis of the function relationship between joint angle and arm angle, and realizes efficient and reliable online avoidance of the joint motion limit of a seven-degree-of-freedom robot arm.

[0007] To solve the above technical problems, the first aspect of the embodiment of the present application provides a SRS configuration robot arm inverse motion control method, comprising the following steps:

[0008] Establishing a function relationship between each of a plurality of joint angles of a seven-degree-of-freedom robot arm and an arm angle, determining a stationary point of each function relationship, and defining the arm angle as the rotation angle of the actual arm plane relative to the reference arm plane around the shoulder-to-wrist connecting line;

[0009] Obtaining the intersection point of each function relationship and the preset angle limit line of the corresponding joint;

[0010] According to the position information of the stationary point and the intersection point, calculating the feasible arm angle interval of the robot arm during motion, and the feasible arm angle interval is the arm angle value range in which all joint angles are within their corresponding angle limit range;

[0011] Selecting a target arm angle according to the feasible arm angle interval, and generating a joint angle control instruction of the robot arm based on the target arm angle.

[0012] Further, the establishment of the function relationship between each of a plurality of joint angles of a seven-degree-of-freedom robot arm and an arm angle comprises:

[0013] Based on the configuration parameters of the robot arm, determining the arm plane with the elbow vertically upward when the arm angle is zero degrees as the reference arm plane, and calculating the virtual joint angle corresponding to the reference arm plane;

[0014] calculating a rotation matrix corresponding to the actual arm plane by Rodrigues rotation formula after the reference arm plane rotates the arm angle around the shoulder-to-wrist connecting line;

[0015] analyzing a function expression of each joint angle with respect to the arm angle from the rotation matrix corresponding to the actual arm plane;

[0016] wherein the function expression of part of the joint angles is an arctangent form containing sine and cosine functions, and the function expression of another part of the joint angles is an inverse cosine form.

[0017] Further, the step of calculating the feasible arm angle interval of the robot arm in the movement process according to the position information of the stationary point and the intersection point comprises:

[0018] judging the distribution form of the function curve of each joint angle with respect to the arm angle relative to the corresponding angle limiting line based on the number of the stationary points and the relative position relationship between the intersection point and the stationary point;

[0019] determining an arm angle sub-interval in which the corresponding joint angle is within the angle limiting range based on the distribution form;

[0020] taking the intersection of the arm angle sub-intervals corresponding to all joints to obtain the feasible arm angle interval.

[0021] Further, the distribution form comprises:

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

[0023] the function curve has two stationary points, and the maximum value point is higher than the upper limit of the joint angle, and the minimum value point is lower than the lower limit of the joint angle;

[0024] the function curve has no stationary point;

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

[0026] Further, when the function curve has one stationary point and the function value at the stationary point has a step change, the step of determining an arm angle sub-interval in which the corresponding joint angle is within the angle limiting range based on the distribution form comprises:

[0027] calculating the left limit value and the right limit value of the function value at the stationary point;

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

[0029] When the left limit value and / or the right limit value exceeds the angle limiting range, according to the intersection position of the function relationship and the angle limiting line, an arm angle interval meeting the joint angle limiting requirement on both sides of the stationary point is selected, and the selected arm angle interval is included in the arm angle sub-interval.

[0030] Further, before the step of establishing the function relationship, the method further comprises:

[0031] setting a global configuration parameter, the global configuration parameter being used to specify a target angle symbol combination of joint two, joint four and joint six;

[0032] based on the target angle symbol combination corresponding to the global configuration parameter, establishing a function relationship between each joint angle and arm angle, the function relationship corresponding to a group of joint angle solutions uniquely determined by the target angle symbol combination.

[0033] Further, the step of selecting a target arm angle according to the feasible arm angle interval comprises:

[0034] based on a historical arm angle value of a previous control period and upper and lower boundaries of the feasible arm angle interval calculated at present, a target arm angle value of a current control period is calculated through a bounded cosine function mapping relationship.

[0035] Further, the bounded cosine function mapping relationship is:

[0036]

[0037] wherein ψ(t) is the target arm angle value of the current control period, ψ(t-1) is the historical arm angle value of the previous control period, ψ all_upper and ψ all_lower are respectively the upper boundary and the lower boundary of the feasible arm angle interval calculated at present, K is a constant for controlling repulsion intensity, and α is a constant for controlling repulsion reaction starting position.

[0038] Correspondingly, a second aspect of the embodiment of the application provides an SRS configuration mechanical arm inverse motion control system, which controls the SRS configuration mechanical arm based on the above-mentioned SRS configuration mechanical arm inverse motion control method, and the control system comprises:

[0039] a stationary point acquisition module, which is used to establish a function relationship between each joint angle of a seven-degree-of-freedom mechanical arm and an arm angle, and determine a stationary point of each function relationship, the arm angle being defined as a rotation angle of an actual arm plane relative to a reference arm plane around a line connecting a shoulder to a wrist.

[0040] an intersection point acquisition module, which is used to acquire an intersection point of each function relationship and a preset angle limiting line of a corresponding joint.

[0041] an arm angle calculation module configured to calculate a feasible arm angle interval of the robot arm in a motion process according to the position information of the stationary point and the intersection point, the feasible arm angle interval being an arm angle value range in which all joint angles are within corresponding angle limit ranges;

[0042] an instruction generation module configured to select a target arm angle according to the feasible arm angle interval, and generate joint angle control instructions of the robot arm based on the target arm angle.

[0043] Correspondingly, a third aspect of the embodiment of the present application provides an electronic device, comprising: at least one processor; and a memory connected with the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the above-mentioned SRS-configuration robot arm inverse motion control method.

[0044] Correspondingly, a fourth aspect of the embodiment of the present application provides a computer readable storage medium having computer instructions stored thereon, the instructions being executed by a processor to implement the above-mentioned SRS-configuration robot arm inverse motion control method.

[0045] The above technical solutions of the embodiment of the present application have the following beneficial technical effects:

[0046] 1. By systematically constructing the functional relationship between the joint angle and the arm angle and analyzing the curve characteristics (such as the stationary point and the intersection with the limit line), the feasible arm angle interval in which all joints are not over-limited can be accurately calculated, thereby fundamentally solving the problems of joint over-limit and trajectory mutation of the seven-degree-of-freedom robot arm in Cartesian space motion, and ensuring the reliability and safety of the motion;

[0047] 2. By introducing a global configuration parameter to pre-lock the desired robot arm configuration, the present application avoids the huge computational overhead of the traditional method of traversing all eight groups of inverse solutions and then screening, and overcomes the computational redundancy and local convergence risk caused by random search of the intelligent optimization algorithm, so that online real-time calculation becomes efficient and the result is determined;

[0048] 3. The adaptive arm angle selection strategy based on the bounded cosine function can smoothly generate the optimal arm angle at the current time according to the arm angle value at the previous time within the feasible arm angle interval, and the output value is naturally bounded, thereby effectively avoiding arm angle jumping and joint speed mutation, and finally generating a continuous, smooth and natural joint trajectory. BRIEF DESCRIPTION OF DRAWINGS

[0049] Figure 1 is a flowchart of the SRS-configuration robot arm inverse motion control method provided by the embodiment of the present application;

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

[0051] Figure 3 is a schematic diagram of a virtual arm plane configuration provided by an embodiment of the present application;

[0052] Figure 4a is a curve schematic diagram of a joint 1 / 3 / 5 / 7-arm angle function having two stationary points provided by an embodiment of the present application Figure 1 ;

[0053] Figure 4b is a curve schematic diagram of a joint 1 / 3 / 5 / 7-arm angle function having two stationary points provided by an embodiment of the present application Figure 2 ;

[0054] Figure 4c is a curve schematic diagram of a joint 1 / 3 / 5 / 7-arm angle function having two stationary points provided by an embodiment of the present application Figure 3 ;

[0055] Figure 4d is a curve schematic diagram of a joint 1 / 3 / 5 / 7-arm angle function having two stationary points provided by an embodiment of the present application

[0056] Figure 5a is a curve schematic diagram of a joint 1 / 3 / 5 / 7-arm angle function having no stationary point provided by an embodiment of the present application Figure 1 ;

[0057] Figure 5b is a curve schematic diagram of a joint 1 / 3 / 5 / 7-arm angle function having no stationary point provided by an embodiment of the present application Figure 2 ;

[0058] Figure 6a is a curve schematic diagram of a joint 1 / 3 / 5 / 7-arm angle function having one stationary point provided by an embodiment of the present application Figure 1 ;

[0059] Figure 6b is a curve schematic diagram of a joint 1 / 3 / 5 / 7-arm angle function having one stationary point provided by an embodiment of the present application Figure 2 ;

[0060] Figure 7a is a curve schematic diagram of a joint 2 / 6-arm angle function having two stationary points provided by an embodiment of the present application Figure 1 ;

[0061] Figure 7b is a curve schematic diagram of a joint 2 / 6-arm angle function having two stationary points provided by an embodiment of the present application 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 is Figure 4d the analysis schematic Figure 3 ;

[0077] Figure 12d is Figure 4d the analysis schematic

[0078] Figure 13 is Figure 5a the classification analysis schematic

[0079] Figure 14a is Figure 5b the analysis schematic Figure 1 ;

[0080] Figure 14b is Figure 5b the analysis schematic Figure 2 ;

[0081] Figure 14c is Figure 5b the analysis schematic Figure 3 ;

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

[0083] Figure 15b is the optimized joint trajectory schematic diagram in the embodiment of the present application

[0084] Figure 16 is the SRS configuration mechanical arm inverse motion control system module block diagram in the embodiment of the present application

[0085] Reference signs:

[0086] 1, a point acquisition module, 2, an intersection acquisition module, 3, an arm angle calculation module, 4, an instruction generation module DETAILED DESCRIPTION

[0087] In order to make the purpose, technical scheme and advantages of the present application more clear and obvious, the present application is further described in detail below in combination with specific embodiments and with reference to the drawings. It should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present application. In addition, in the following description, the description of the known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present application.

[0088] For the redundant manipulator of SRS configuration, there are infinite inverse solutions corresponding to the same end pose, and the null space characteristics of the infinite solutions are related to the selection of the arm angle, and different arm angles correspond to different solution spaces. The arm angle of the manipulator of SRS configuration and the angle of each joint are in a function relationship only with first type discontinuity points (jump discontinuity points and removable discontinuity points), and by analyzing the intersection relationship between the stationary points of the functions and the upper and lower limit lines of each joint, an arm angle interval in which the joint is within the limit range can be obtained, and then a better arm angle is selected in the arm angle interval through a preset strategy, and finally the inverse kinematics is calculated using the arm angle to obtain the inverse solution satisfying the joint limit.

[0089] Please refer to Figure 1 To achieve the above object, a first aspect of an embodiment of the present application provides a control method for inverse kinematics of a manipulator of SRS configuration, comprising the following steps:

[0090] Step S100, a function relationship between each joint angle of the seven-degree-of-freedom manipulator and the arm angle is established, and the stationary points of each function relationship are determined. 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.

[0091] The function relationship between each joint angle of the seven-degree-of-freedom manipulator and the arm angle is established, and the stationary points are determined. Firstly, according to the configuration definition of the DH parameters of the manipulator, the arm plane corresponding to the manipulator configuration with the elbow vertically upward at zero degrees is determined as the reference arm plane, and the angle values of each virtual joint in this specific configuration are calculated. Then, the rotation matrix of the actual arm plane corresponding to the rotation of the reference arm plane around the line connecting the shoulder S to the wrist W by any arm angle ψ is calculated through the Rodrigues rotation transformation formula. Starting from the rotation matrix of the actual arm plane, the explicit function expressions of the angles of joints 1, 2, 3, 5, 6 and 7 with respect to the arm angle ψ are finally derived by simultaneously solving the rotation matrix and the forward kinematics equation of the manipulator and algebraically eliminating. These function expressions are specifically expressed in two forms: for joints 1, 3, 5 and 7, the expressions are arctangent functions containing linear combinations of sine function sin(ψ) and cosine function cos(ψ) in the numerator and denominator; for joints 2 and 6, the expressions are arccosine functions containing linear combinations of sin(ψ) and cos(ψ). After obtaining these function relationships, all stationary points of each function in its domain are solved by taking the derivative and setting the derivative to zero.

[0092] Step S200, the intersection points of each function relationship and the preset angle limit line of the corresponding joint are obtained.

[0093] To find all intersection points of each joint angle function curve and the upper and lower limit level lines representing the physical motion limits of the joint, the joint angle value is set to its upper limit value q max or lower limit value q min , and substituted into the function relationship θ i(ψ) obtained in step S100 to inversely solve the arm angle value ψ at this time. This solving process involves solving a trigonometric function equation about sin(ψ) and cos(ψ), and the number of solutions (0, 1 or 2) depends on the relative position relationship between the function curve and the limit line. All the solutions represent the intersection arm angle values of the function curve and the limit line, and are the key boundary points for subsequent feasibility interval judgment.

[0094] In step S300, the feasible arm angle interval of the robot arm in the motion process is calculated according to the position information of the stationary points and the intersection points. The feasible arm angle interval is the arm angle value range in which all joint angles are within their corresponding angle limit ranges.

[0095] First, the function curve shape of each joint is classified and distinguished by comprehensively considering the number, type (maximum value point or minimum value point) and relative distribution position of the stationary points of each joint function and the intersection points with the upper and lower limit lines. Typical shapes include but are not limited to: a shape with two stationary points and both extreme value points within the limit range, a shape with two stationary points and both extreme value points outside the limit range, a monotonic shape without stationary points, and a singular shape with only one stationary point and a π radian jump in the function value at the point. For each distinguished shape, one or more arm angle sub-intervals that guarantee the joint angle not to exceed the limit are determined according to the comparison results of the stationary point function value and the joint limit value, using specific interval selection rules. Finally, the allowed arm angle sub-intervals calculated for all joints are taken as the logical intersection, thereby obtaining the global feasible arm angle interval ψ all .

[0096] In step S400, a target arm angle is selected according to the feasible arm angle interval, and joint angle control instructions of the robot arm are generated based on the target arm angle.

[0097] After obtaining the global feasible arm angle interval [ψ all_lower , ψ all_upperAfterwards, a specific arm angle value ψ(t) needs to be selected from the feasible arm angle interval for the inverse kinematics calculation in the current control cycle. The selection strategy needs to take into account both safety and motion smoothness: taking the arm angle value ψ(t-1) adopted in the previous control cycle as the input, and taking the relative distance of the input to the boundary of the feasible arm angle interval as the variable, a designed bounded cosine function is used for mapping calculation. The function can generate an adjustment amount biased towards the center of the feasible arm angle interval, ensuring that the output value ψ(t) is strictly within the feasible arm angle interval, and its smoothness ensures the continuity of the arm angle change. 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 the values are converted into control instructions for the driving motor to execute.

[0098] By establishing the exact function relationship between the joint angles and the arm angle and using its mathematical properties for systematic analysis, the feasible arm angle working interval that strictly satisfies all joint angle limit constraints when the manipulator executes a Cartesian space trajectory can be calculated online and automatically, and based on this, the optimal arm angle is selected using a smooth adaptive strategy, and finally a safe, continuous and non- abrupt high-quality joint trajectory is generated, which fundamentally solves the technical problems of joint over-limiting and low calculation efficiency or unstable results of traditional solution methods in the process of trajectory tracking of a seven-degree-of-freedom redundant manipulator.

[0099] Further, the function relationship between each of the joint angles and the arm angle of the seven-degree-of-freedom manipulator in step S100 includes:

[0100] In step S110, based on the configuration parameters of the manipulator, the arm plane with the elbow vertically upward when the arm angle is zero degrees is determined as the reference arm plane, and the virtual joint angles corresponding to the reference arm plane are calculated.

[0101] For a seven-degree-of-freedom manipulator with an SRS configuration, the state of the elbow vertically upward in the initial configuration of the manipulator is taken as the reference arm plane, which is determined by the positions of the shoulder, elbow and wrist. Through the DH parameters of the manipulator and the current end pose, the vector from the base coordinate system to the wrist can be calculated, and then the virtual joint angles, including the angles of virtual joints 1, 2, 3 and 4, are solved according to the geometric relationship. These virtual joint angles are used to represent the configuration of the manipulator in the reference arm plane, providing a basis for subsequent arm angle transformation.

[0102] In step S120, the rotation matrix corresponding to the actual arm plane obtained by rotating the reference arm plane around the line connecting the shoulder to the wrist by the arm angle is calculated by the Rodrigues rotation transformation formula. The function expression of part of the joint angles is in the form of arctangent containing sine and cosine functions, and the function expression of the other part of the joint angles is in the form of inverse cosine.

[0103] Based on the virtual joint angles obtained in the reference arm plane, a rotation matrix in the reference arm plane is calculated by forward kinematics; subsequently, the rotation matrix in the reference arm plane is rotated and transformed by using the Rodrigues rotation formula with the vector from the shoulder to the wrist as the rotation axis, so as to obtain a rotation matrix in the actual arm plane. The rotation matrix can be decomposed into a linear combination of sine and cosine functions of the arm angle, and the function expressions of part of the joint angles are expressed in the arctangent form containing the sine and cosine functions, and the other part of the joint angles are expressed in the inverse cosine form, so as to establish an explicit function relationship between the joint angles and the arm angle.

[0104] In step S130, the function expressions of the joint angles with respect to the arm angle are analyzed 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 equation of the robot arm, the function expressions of joints 1, 2, 3, 5, 6 and 7 with respect to the arm angle can be extracted respectively. These function expressions have a clear mathematical form, including arctangent functions and inverse cosine functions, and the coefficients are determined by the configuration parameters of the robot arm and the current end pose, so as to accurately reflect the relationship between the joint angles and the arm angle, and provide a mathematical model basis for subsequent joint limit analysis and arm angle optimization.

[0106] By establishing the function relationship between the joint angles and the arm angle, and based on the relationship, the arm angle interval is solved and optimized, which can effectively avoid the joint exceeding the limit during the movement of the robot arm, improve the trajectory smoothness and motion reliability, and has good calculation efficiency and practicability.

[0107] Specifically, the step of calculating the feasible arm angle interval of the robot arm in the movement process according to the position information of the stationary point and the intersection point in step S300 includes:

[0108] In step S310, the distribution form of the function curve of each joint angle with respect to the arm angle relative to the corresponding angle limit line is judged according to the number of stationary points and the relative position relationship between the intersection point and the stationary point.

[0109] For each function of the joint angle with respect to the arm angle, first, the number of derivative zero points is determined to determine whether the function curve has zero, one or two stationary points; then, the intersection position of the function curve and the joint limit line is obtained by solving the equation that the joint angle function value is equal to the upper limit or lower limit value; further, the specific form category of the function curve is accurately judged, such as monotonically increasing, monotonically decreasing, or having one or more extreme points, etc., by comprehensively considering the properties of the stationary points (maximum points or minimum points), the number of intersection points and the relative distribution of the two in the entire arm angle definition domain, so as to provide a basis for subsequent interval division.

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

[0111] According to the curve form determined in step S310, combined with the intersection value of the upper and lower limit lines calculated, by analyzing the size relationship between the function value in different segments of the arm angle definition domain and the joint limit value, the arm angle interval segments that can satisfy the joint angle limit are determined in a logical judgment manner; these interval segments may be continuous intervals, or the union of multiple discontinuous intervals, and are finally defined as the feasible arm angle sub-interval corresponding to the joint.

[0112] Step S330, take the intersection of all joint corresponding arm angle sub-intervals to get the feasible arm angle interval.

[0113] Take the intersection of all joint corresponding arm angle sub-intervals to get the feasible arm angle interval that satisfies all joint limit constraints at the current pose of the robot arm. Perform set intersection operation on the feasible arm angle sub-intervals corresponding to joints 1, 2, 3, 5, 6, and 7 calculated in step S320; the result of the intersection operation is one or more continuous arm angle intervals, which represents the arm angle value range that ensures all related joints within their angle upper and lower limit ranges, providing a feasible selection range for the final selection of an optimal arm angle.

[0114] By systematically analyzing the function relationship curve form of each joint angle and arm angle, and accurately calculating the feasible arm angle sub-interval of each joint, the global feasible arm angle interval is finally obtained by taking the intersection, which can effectively ensure that all joint angles calculated by inverse solution are strictly within the movement limit of the robot arm body, thereby ensuring the feasibility, safety and smoothness of trajectory execution, and avoiding motion interruption or mechanism damage caused by joint over-limit.

[0115] Further, the distribution form in step S320 includes the following cases:

[0116] First, the function curve has two stationary points, and the maximum value point is lower than the joint angle upper limit, and the minimum value point is higher than the joint angle lower limit. The first distribution form refers to the case that the function curve has two stationary points, and the maximum value point is lower than the joint angle upper limit, and the minimum value point is higher than the joint angle lower limit. In this case, the overall fluctuation range of the function curve is completely within the range defined by the joint angle upper and lower limits, and both extreme values do not reach the limit line, so the entire arm angle definition domain, i.e. the continuous interval from negative π to positive π, is the feasible arm angle sub-interval of the joint, without the need for interval truncation.

[0117] Second, the function curve has two stationary points, and the maximum value point is higher than the upper limit of the joint angle, and the minimum value point is lower than the lower limit of the joint angle. The second distribution form refers to the case that the function curve has two stationary points, and the maximum value point is higher than the upper limit of the joint angle, and the minimum value point is lower than the lower limit of the joint angle. At this time, the function curve exceeds the upper limit and the lower limit of the joint at the same time, and intersects with two joint limiting lines to produce four intersection points; based on the relative position relationship between these intersection points and the two stationary points, logical judgment can be made to determine that the arm angle interval in which the function value is within the permitted range is usually composed of three discontinuous interval segments, that is, from negative π to the first intersection point with the upper limit, from the second intersection point with the upper limit to the first intersection point with the lower limit, and from the second intersection point with the lower limit to positive π.

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

[0119] Fourth, the function curve has one stationary point, and the function value at the stationary point changes by a step. The fourth distribution form refers to the case that the function curve has one stationary point, and the function value at the stationary point changes by a step. This case is caused by the mathematical singular condition, which shows that the function jumps by π radians at the stationary point, thereby dividing the entire arm angle domain into two monotonic intervals at the stationary point; the intersection points of the function and the joint limiting lines need to be solved in the two monotonic intervals, and based on the limit values on both sides of the jump point and the positions of the intersection points, it is determined that the two possible discontinuous interval segments together constitute the feasible arm angle sub-interval of the joint.

[0120] By systematically identifying and judging the above four typical function curve distribution forms, and accurately calculating the feasible arm angle sub-interval of each joint, the global feasible arm angle interval is provided for the final intersection obtained by the method, which ensures that the inverse solution calculation result satisfies all joint limiting constraints.

[0121] Further, when the function curve has one stationary point and the function value at the stationary point changes by a step, the determination of the arm angle sub-interval in which the corresponding joint angle is within the angle limiting range based on the distribution form in step S320 comprises:

[0122] In step S321, the left limit value and the right limit value of the function value at the stationary point are calculated.

[0123] Based on the function expression of the joint angle with respect to the arm angle, the limits are obtained by approaching the stationary point from the left and right sides respectively. Since the function has a step change at the point, the left limit and the right limit value are usually not equal, and the difference is about π radians. The two limit values represent 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 the point and whether the joint limit is met.

[0124] Step S322, when the left limit value and the right limit value are both within the angle limit range of the corresponding joint, the stationary point is excluded from the arm angle sub-interval.

[0125] Although the limit values on both sides of the stationary point meet the requirements, since the function is not defined at the stationary point or has a sudden change, the value may be uncertain or exceed the limit, so the point cannot be included in the feasible interval. At this time, the feasible arm angle sub-interval is composed of two continuous interval segments on the left and right sides of the stationary point, i.e. 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 exceeds the angle limit range, according to the intersection position of the function relationship and the angle limit line, the arm angle interval on both sides of the stationary point that meets the joint angle limit requirement is selected, and the selected arm angle interval is included in the arm angle sub-interval.

[0127] Specifically, the equations of the function value equal to the upper and lower limits of the joint angle are solved in the monotonic interval on the left side and the monotonic interval on the right side of the stationary point respectively to obtain the intersection points. Then, according to the specific situation of the limit value exceeding the limit (for example, only the left side exceeds or only the right side exceeds or both sides exceed) and the position of the intersection point, the arm angle interval segments whose function value continuously remains within the limit range are selected from the intervals on both sides, and these interval segments are included in the feasible arm angle sub-interval of the joint.

[0128] Through the above special processing of the function curve with a step singularity point, all feasible arm angle segments that meet the joint limit constraint in such a case can be accurately and completely determined, ensuring the completeness and accuracy of the subsequent interval intersection operation, thereby providing a solid foundation for safe and smooth motion planning of the robot arm.

[0129] Further, before establishing the function relationship in step S100, it further includes:

[0130] Step S101, setting global configuration parameters, the global configuration parameters are used to specify the target angle symbol combination of joint two, joint four and joint six.

[0131] For multiple inverse kinematics solutions existing in the same end pose, by pre-defining the expected signs (positive or negative) of the three key joint angles, a group of target solution configurations can be uniquely determined; the global configuration parameter is usually set according to the sign state of the joint angle solution adopted in the last motion period during the continuous motion of the robot arm, so as to ensure that the current solution and the configuration at the last time are consistent in sign, maintain the continuity and consistency of the motion trajectory, and provide a prerequisite for subsequent establishment of a certain function relationship.

[0132] In step S102, a function relationship between each joint angle and the arm angle is established based on the corresponding target angle sign combination of the global configuration parameter, and the function relationship corresponds to a group of joint angle solutions uniquely determined by the target angle sign combination.

[0133] When constructing the function relationship, the specified joint sign combination is substituted into the inverse kinematics derivation process based on the arm angle parameterization as a known condition, so that each coefficient in the final function expression of the joint angle with respect to the arm angle (including the inverse tangent function and the inverse cosine function form) is associated with the sign combination, thereby ensuring that the established function relationship only describes the solution branch under a specific sign configuration, avoiding the calculation redundancy caused by solving all eight possible solutions.

[0134] By pre-setting the global configuration parameter and establishing the corresponding function relationship, the entire inverse solution calculation process is always carried out around a group of determined and continuous solution branches with the configuration at the last time, which not only significantly improves the calculation efficiency and avoids the huge calculation overhead of traversing all solution branches, but more importantly, guarantees the sign consistency of the joint angle solution in the continuous trajectory tracking process, thereby effectively preventing the joint angle mutation and the violent shaking of the robot arm caused by the solution branch jump, and laying a stable and reliable foundation for subsequent joint limit check and arm angle optimization.

[0135] Further, the target arm angle is selected according to the feasible arm angle interval in step S400, including:

[0136] In step S410, the target arm angle value of the current control period is calculated by the bounded cosine function mapping relationship based on the historical arm angle value of the last control period and the upper and lower boundaries of the feasible arm angle interval calculated at present.

[0137] The actual arm angle value adopted at the previous moment is taken as input and compared with the lower limit value and the upper limit value of the current feasible arm angle interval; based on the comparison result, an arm angle adjustment amount is calculated by using a mathematical mapping relationship taking a cosine function as the core and having a bounded output value; the adjustment amount will make the new arm angle value approach the center of the current feasible interval, but the adjustment range is restricted by the natural boundedness of the cosine function, avoiding approaching or crossing the boundary of the feasible interval due to excessive adjustment; wherein, by adjusting the gain coefficient and the scale factor in the function, the convergence speed of the arm angle approaching the center of the interval and the critical distance at which the significant adjustment begins can be flexibly controlled, so as to ensure the calculation efficiency while smoothly generating a target arm angle value that meets all joint position constraints and is located as centrally as possible in the feasible interval. By adopting the adaptive arm angle selection scheme based on the bounded cosine function, the target arm angle at the current moment can be smoothly and controllably generated from the arm angle state at the previous moment within the known feasible arm angle interval, and the process is accurate and efficient, effectively avoiding the shaking or mutation of the arm angle near the boundary of the interval, and the inherent boundedness ensures that the output arm angle will never exceed the given feasible interval range, thereby providing a key guarantee for finally solving a set of safe, stable and continuous joint angle solutions.

[0138] Further, the bounded cosine function mapping relationship is:

[0139]

[0140] wherein ψ(t) is the target arm angle value of the current control period, ψ(t-1) is the historical arm angle value of the previous control period, ψ all_upper and ψ all_lower are the upper boundary and the lower boundary of the current calculated feasible arm angle interval, respectively, K is a constant controlling the repulsion strength, and a is a constant controlling the starting position of the repulsion reaction.

[0141] The mapping relationship takes the historical arm angle value as the starting point of calculation and compares it with the median value of the current feasible interval; its core construction is to use the periodicity, smoothness and boundedness of the cosine function to generate an adjustment amount with limited amplitude. The size of the adjustment amount depends on the relative position of the historical value in the feasible interval: when the historical value is close to the center of the interval, the adjustment amount tends to zero, keeping the arm angle stable; when the historical value deviates from the center and approaches either boundary, the adjustment amount will significantly increase, pointing in the direction of the center of the interval, thereby driving the arm angle away from the boundary. Among them, the constant K is used to globally adjust the strength of the center-seeking behavior, and the constant a is used to control the critical position at which the adjustment behavior begins to become significant, i.e., to define a "dead zone" range around the center of the interval. The output of the entire mapping relationship is strictly limited within a bounded range, ensuring that the target arm angle value calculated finally will never exceed the given feasible arm angle interval.

[0142] Further, based on the arm angle selection scheme of bounded function mapping, by using the mathematical characteristics of smooth continuous function, a target arm angle value located inside the interval and tending to the central region can be adaptively generated according to the arm angle state at the last moment and the current feasible interval; the method not only has efficient calculation process, but more importantly, the output result can absolutely guarantee not to exceed the safety region defined by the joint limit, and at the same time, frequent jitter or sharp jump of the arm angle value near the interval boundary can be effectively avoided.

[0143] Next, the above control method is described in detail in a specific manner:

[0144] Firstly, since each arm angle value corresponds to 8 groups of effective solutions of the mechanical arm, the 8 groups of solutions can be distinguished according to the signs of joint 2, joint 4 and joint 6. A sign configuration parameter Gk i (i = 2, 4, 6) is defined for distinguishing the 8 groups of solutions in the calculation process, and the specific definition is as follows:

[0145]

[0146] For the SRS configuration redundant mechanical arm, the elbow E can rotate around the vector between the shoulder and the elbow to form infinite groups of solutions of the redundant mechanical arm. As shown in Figure 1 , wherein the plane WE v S is the reference arm plane, WES is the arm plane where the actual configuration is located, and ψ is the arm angle. Figure 2 is the virtual arm plane calculation diagram, wherein x-y-z 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 inverse kinematics is performed, the given end pose can be solved by the following formula to obtain the wrist position:

[0150]

[0151] Further, the vector p 26 is calculated by the following formula:

[0152]

[0153] Since the length of the vector and the vector is the fixed parameters d3 and d5 of the mechanical arm link, the virtual joint can be calculated by the cosine theorem:

[0154]

[0155] Similarly, Figure 2 φ can also be calculated by the cosine theorem:

[0156]

[0157] Then, by the internal angle theorem, the virtual joint can be obtained by the following formula, where p26(x), p26(y), p26(z) are the components of the vector p 26 The 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 take the arm plane corresponding to the vertical upward configuration of the elbow of the robot arm as the reference arm plane. The rotation of joint 3 directly affects the shape of the elbow of the robot arm, so the 0 position is taken as the configuration of the virtual joint .

[0160] Since the vector p 26 The angle between the projection vector of the base coordinate system x-y plane and the x-axis component x of the base coordinate system is the virtual joint 1 which can be given by the following formula.

[0161]

[0162] At this point, the virtual joints in the virtual arm plane have been obtained. At this point, substitute into the forward kinematics equation to obtain the forward kinematics equation corresponding to the virtual arm plane From which the rotation matrix

[0163] It can be seen from Figure 1 that the actual arm plane WES is obtained by rotating the reference arm plane WE v S around the vector by the arm angle ψ. The rotation matrix Both are composed of three groups 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 to transform to obtain the actual arm plane corresponding to the virtual arm plane after rotating the vector by the arm angle ψ This procedure is given by the following equation:

[0164]

[0165] Further simplifying the above equation gives:

[0166]

[0167] Knowing the above, the actual arm plane corresponding θ1, θ2, θ3 can be solved by algebraic method through the following equation.

[0168]

[0169] Where,

[0170]

[0171] From the above equation, The following equation can be obtained From the following equation,

[0172]

[0173] Further simplifying the above equation gives

[0174]

[0175] Knowing the above, the actual arm plane corresponding θ5, θ6, θ7 can be solved by algebraic method through the following equation.

[0176]

[0177] From the above simplified equation:

[0178]

[0179] The following equation can be obtained:

[0180]

[0181] Substituting the above equation into the solving equation of θ1, θ2, θ3, θ5, θ6, θ7, the following equation can be obtained, which is the function of the angle of joint 1, 3, 5, 7 and joint 2, 6 relative to the arm angle ψ.

[0182]

[0183] From the function properties of arcos and atan2, it can be known that the function equation only has the first type of discontinuous points, i.e. removable discontinuous points and jump discontinuous points, in the definition domain, and the function is smooth and continuous outside the discontinuous points. This provides the possibility for determining the arm angle interval that makes the joint within the limit.

[0184] Next, the above two formulas are differentiated with respect to the arm angle ψ, and the chain rule is used to derive the following formula. Here, only the formulas corresponding to joints 1, 2, and 3 are given, and the formulas corresponding to joints 5, 6, and 7 are similarly derived.

[0185]

[0186]

[0187] When the joints 1 and 3 are considered, let θ i ′(ψ) = 0 to obtain the function stationary point, as shown in the following formula. The formulas corresponding to joints 5 and 7 are similarly derived.

[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] From the above formula, it can be found that the number and properties of the function stationary points can be analyzed according to the formula K at 2 +K bt 2 -K ct 2

[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 >0 (i.e. there are two stationary points), the function curve is as shown in Figure 4a 、 Figure 4b 、 Figure 4c and Figure 4d .

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

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

[0208] When θ i ′(ψ) =0 for joint 2, the function stationary point is obtained as follows. The formula corresponding to joint 6 is similarly derived.

[0209]

[0210] From the above formula, it can be found that the number and properties of the function stationary points can be analyzed according to the formula K a 2 +K b 2 under the square root.

[0211] When K a 2 +K b 2 >0, i.e. there are two stationary points of the function;

[0212] When K a 2 +K b 2 =0, i.e. there is only one stationary point of the function.

[0213] Through experiments, it can be found 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 stationary 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 θ i The inverse function ψ(θ i ) is given as follows.

[0221] For joint 1, 3, ψ(θ i ) is: For joint 5, 7, ψ(θ i ) is similarly derived

[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] From the above equations we have:

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

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

[0229] If K bp 2 - 4K ap K cp < 0, then ψ(θ i ) has no possible value, corresponding to no intersection point.

[0230] For joint 2, ψ(θi ) is: (the ψ(θ i ) of joint 6 is similar derivation),

[0231]

[0232] According to the above formula:

[0233] If K a 2 +K b 2 -(K c -cos(θ i )) 2 =0, ψ(θ 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, ψ(θ i ) has two possible values, corresponding to two intersection points.

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

[0236] Then, the curve profile of joints 1, 3, 5, and 7 is analyzed.

[0237] There are two cases of stationary points:

[0238] For Figure 4a : When the case of Figure 9a occurs, i.e., the upper and lower limits of the joint have no intersection with the function curve, the feasible arm angle interval is [-π, π].

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

[0240] When the case of Figure 9c occurs, θ i (ψ 0_right ) ≥ qmin and θ i (ψ 0_left ) > qmax, the feasible arm angle interval is [-π, ψ intermax1 ]∪[ψ intermax2 , π]. The other reverse symmetry cases are similar.

[0241] For Figure 4b : When the case of Figure 4b occurs, and there is an intersection with the upper and lower limits of the joint, there may be a conflict with the judgment logic of Figure 4a , at this time, the first derivative values on both sides of the two stationary points ψ 0_right and ψ 0_left need to be calculated to determine whether it is a maximum value point or a minimum value point. The first type of discontinuity in Figure 4b occurs when θ i (ψ) is on the boundary of -π or π, at this time, the corresponding midpoint of -π or π and the two extreme points can be calculated, and the corresponding case of FIG. 10 is calculated and (other reverse symmetry cases are similarly derived). At this time, the joint limit line passing through the midpoint must have two intersection points with the curve, and the intersection points ψ intermax1 , ψ intermax2 , ψ intermin1 , ψ intermin2 are calculated, if ψ intermax1 , ψ intermax2 and ψ intermin1 , ψ intermin2 are distributed on both sides of the corresponding ψ 0_right and ψ 0_left and the maximum value point is less than the minimum value point, it is judged that this curve profile of Figure 4b has entered, and the corresponding analysis is performed next.

[0242] When the case of Figure 10a occurs, that is, the upper and lower limits of the joint have no intersection with the function curve, the feasible arm angle interval is [-π, π].

[0243] When the case of Figure 10b occurs, the boundary values of θ i (-π) and θ i (π) are calculated. If θ i (-π) and θ i (π) are not in [qmin, qmax], the feasible arm angle interval is [ψ intermax1 , ψ intermax2 ]∪[ψintermin1 , ψ intermin2 ].

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

[0245] For Figure 4c : When Figure 4c this case occurs, it will conflict with Figure 4a and Figure 4b , a way to determine this type of curve is needed, which is similar to analyzing the curve profile of Figure 4b . The first type of discontinuity in Figure 4c also occurs at θ i (ψ) is on the boundary of -π or π, at this time, -π or π can be calculated respectively with the corresponding midpoint of the two extreme points, and the corresponding case of FIG. 11 is calculated and (other reverse symmetric cases are similarly derived). At this time, the joint limit line passing through the midpoint will have two intersection points with the curve, and the intersection points ψ intermax1 , ψ intermax2 , ψ intermin1 , ψ intermin2 of the corresponding midpoint and the function curve are calculated. If ψ intermax1 , ψ intermax2 and ψ intermin1 , ψ intermin2 exist a group that is not distributed on both sides of the corresponding ψ 0_right and ψ 0_left , and the maximum point is less than the minimum point, it is judged that this curve profile of Figure 4c has entered, and the corresponding analysis is performed next.

[0246] When the case of Figure 11a occurs, that is, the upper limit of the joint has no intersection with the function curve, the feasible arm angle interval is [-∏, π].

[0247] When the case of Figure 1 1b occurs, ψ intermin1 , ψ intermin2 , ψ intermax1 , ψ intermax2 and ψ 0_left , ψ 0_right are calculated.

[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 the analysis of the function curve of joints 1, 3, 5, 7 and the joint limit. Figure 4a Similarly, detailed derivation is not performed here.

[0263] After the above analysis, the feasible arm angle interval satisfying the corresponding joints 1, 3, 5, 7, 2, 6 without exceeding the joint limit is obtained, denoted as

[0264] Then, ψ is obtained denoted as ψ all . ψ all is the total intersection ψ of the feasible arm angle interval satisfying the corresponding joints 1, 3, 5, 7, 2, 6 without exceeding the joint limit all =[ψ all_lower ,ψ all_upper ].

[0265] Finally, the arm angle selection function based on the bounded cosine function is selected in the total intersection ψ all , which makes the arm angle be in the center position of the total intersection, and inverse kinematics is solved.

[0266] where K is a constant, controlling the repulsion strength of the arm angle relative to the boundary ψ of the total intersection all_lower , ψ all_upper , and α is a constant, controlling the limit distance at which the arm angle starts to repel.

[0267]

[0268] The application is mainly used in the inverse solution process of Cartesian motion of the end of the mechanical arm, and the application process steps are as follows:

[0269] In a Cartesian motion process, the current joint angle θ ref is first obtained, and the homogeneous transformation matrix of the end of the mechanical arm corresponding to θ ref is obtained through forward kinematics. Then, the target end pose homogeneous transformation matrix of the next time is obtained according to the trajectory planning result. The arm angle ψ(t-1) selected in the solution process of the last time is known, and through arm angle method analysis, the function relationship between the joint angle θ i and the arm angle ψ when the end pose is is obtained. After obtaining the function relationship, the curve profile corresponding to each joint is analyzed, and then the intersection of the curve profile and the upper and lower limit lines of each joint is analyzed to obtain the feasible arm angle interval of each joint without exceeding the joint limit. Finally, the total feasible arm angle interval ψ all, then substitute into the adaptive arm angle generation function based on the cosine function to obtain the optimal arm angle ψ(t) that satisfies the joint limit at the current time and perform arm angle method kinematics solving to obtain the joint angle solution at the current time.

[0270] Step S1: Obtain the joint angles θ t-1 at time t-1, and obtain the corresponding homogeneous transformation matrix

[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 angle θ corresponding to the above reference arm plane under the end pose can be obtained. and the actual arm plane corresponding to the reference arm plane rotating the arm angle ψ is obtained. and After eliminating the following forward kinematics equation and establishing an equation relationship with the above two equations.

[0273]

[0274]

[0275] and the function relationship between the joint angle θ i and the arm angle ψ is obtained.

[0276]

[0277] Step S4:

[0278] The above formula is derived with respect to the arm angle ψ to obtain (similar derivation for joints 5, 7, and 6).

[0279]

[0280] Make the derivative expression equal to 0 to obtain the curve contour stationary point solving formula of joints 1 and 3 (similar derivation for joints 5 and 7).

[0281]

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

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

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

[0285] The formula of finding the stationary point of the curve profile of joint 2 (similar derivation for joint 6)

[0286]

[0287] Step S5: After obtaining the formula of finding the stationary point, the curve profile analysis can be carried out. First, the state and shape of the curve profile are determined through 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 > 0: that is, the function has two stationary points.

[0289] When K at 2 + K bt 2 - K ct 2 < 0: that is, the function has no stationary point.

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

[0291] When K a 2 + K b 2 > 0: that is, the function has two stationary points.

[0292] When K a 2 + K b 2 = 0: that is, the function has only one stationary point. According to the relationship between the value of the function curve at the stationary point and the upper and lower limit values of the corresponding joint, the arm angle interval that can make the corresponding joint be between the upper and lower limit values is determined Finally, the intersection of these feasible arm angle intervals is obtained to obtain the total feasible arm angle interval ψ that can simultaneously make joints 1, 2, 3, 5, 6 and 7 within the upper and lower limit ranges all

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

[0294]

[0295] Step S7: Substitute ψ(t) into the following function equation to obtain the joint angle value corresponding to the arm angle ψ(t) and issue the inverse solution of the mechanical arm at the t time Since the arm angle is selected within the total feasible arm angle interval, the joint angle value obtained can be within the joint limit range.

[0296]

[0297] Next, a kinematic model of a seven-axis mechanical arm is used to perform a Cartesian space motion test to verify whether the method of the present application can avoid joint limits. The DH parameters and joint limit parameters are shown in the following table.

[0298]

[0299]

[0300] In the experimental part of the present application, a set of relatively extreme initial configurations [160°, -20°, 50°, 110°, -40°, 120°, 160°] are set for the mechanical arm, and the initial pose transformation matrix is:

[0301]

[0302] It can be found from the set of initial configurations that joints 1, 6 and 7 have approached the joint limit. Now set the pose of the mechanical arm end to be unchanged, and move the position along the x and y axes by 0.15m respectively, determine the global configuration parameter Gk at the current time according to the solution at the last time i and obtain a set of inverse solutions after arm angle optimization. In the verification process, a general fixed arm angle selection method is added for comparison test. The joint trajectory comparison is shown in Figure 15a and Figure 15b .

[0303] It can be seen that 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 process, 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 the present invention aim to protect a reverse motion control method and system for an SRS-configured robotic arm, which has the following effects:

[0312] 1. By systematically constructing the function relationship between joint angle and arm angle and analyzing the curve characteristics (such as stationary point, intersection with limit line), the feasible arm angle interval in which all joints are not over-limited can be accurately calculated, which fundamentally solves the problem of joint over-limit and trajectory mutation in Cartesian space motion of seven-degree-of-freedom manipulator, and ensures the reliability and safety of motion;

[0313] 2. By introducing global configuration parameters to pre-lock the desired manipulator configuration, the present application avoids the huge calculation overhead of traditional method which needs to traverse all eight groups of inverse solutions and then filter, and also overcomes the calculation redundancy and local convergence risk caused by random search of intelligent optimization algorithm, so that online real-time calculation becomes efficient and the result is determined;

[0314] 3. The adaptive arm angle selection strategy based on bounded cosine function can generate the optimal arm angle at the current time from the arm angle value at the last time within the feasible arm angle interval, and the output value is naturally bounded, so as to effectively avoid arm angle jumping and joint speed mutation, and finally generate continuous, smooth and natural joint trajectory.

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

[0316] The present application is described with reference to flowcharts and / or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, as well as combinations of flows and / or blocks in the flowcharts 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, a special-purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing apparatus produce a means for implementing the functions specified in the flowcharts and / or block diagrams. Figure 1 The means for implementing each flow or multiple flows and / or blocks Figure 1 The means for implementing each flow or multiple flows and / or blocks

[0317] These computer program instructions can also be stored in a computer-readable memory that can direct the computer or other programmable data processing apparatus to work in a specific manner, so that the instructions stored in the computer-readable memory produce a product including instruction means, which implement the functions specified in the flowcharts and / or block diagrams. Figure 1 The means for implementing each flow or multiple flows and / or blocksFigure 1 the function specified in the one or more blocks.

[0318] These computer program instructions can also be loaded into computer or other programmable data processing devices, so that a series of operation steps are performed on the computer or other programmable data processing devices to generate computer-implemented processes, thus the instructions executed on the computer or other programmable data processing devices provide processes for implementing the flows Figure 1 the flows or the plurality of flows and / or blocks Figure 1 the steps of the function specified in the one or more blocks.

[0319] Finally, it should be noted that: the above examples are only used to illustrate the technical solutions of the present application, but not to limit it, although the above embodiments of the present application have been described in detail, those skilled in the art should understand: the specific embodiments of the present application can be modified or replaced by the same, without departing from the spirit and scope of the present application, any modification or equivalent replacement, which should be covered within the scope of protection of the claims of the present application.

Claims

1. A method for inverse kinematics control of a SRS configuration robotic arm, characterized in that, The method comprises the following steps: establishing a function relationship between each joint angle of a seven-degree-of-freedom robot arm and an arm angle, the arm angle being defined as the rotation angle of an actual arm plane relative to a reference arm plane about a line connecting a shoulder to a wrist; obtaining an intersection point of each function relationship and a preset angle limit line of the corresponding joint; calculating a feasible arm angle interval of the robot arm in a movement process according to position information of the stationary points and the intersection points, the feasible arm angle interval being an arm angle value range in which all joint angles are within their corresponding angle limit ranges; selecting a target arm angle according to the feasible arm angle interval, and generating a joint angle control instruction of the robot arm based on the target arm angle.

2. The SRS configuration robot inverse kinematics control method of claim 1, wherein, The establishment of the function relationship between each joint angle of the seven-degree-of-freedom robot arm and the arm angle comprises: determining, based on configuration parameters of the robot arm, that an arm plane in which the elbow is vertically upward when the arm angle is zero degrees is the reference arm plane, and calculating a virtual joint angle corresponding to the reference arm plane; calculating, by using a Rodrigues rotation transformation formula, a rotation matrix of an actual arm plane obtained after the reference arm plane is rotated about the line connecting the shoulder to the wrist by the arm angle; analyzing, from the rotation matrix of the actual arm plane, a function expression of each joint angle with respect to the arm angle; wherein the function expression of part of the joint angles is an arctangent form containing sine and cosine functions, and the function expression of another part of the joint angles is an inverse cosine form.

3. The SRS-configuration manipulator inverse kinematics control method of claim 1, wherein, The calculation of the feasible arm angle interval of the robot arm in the movement process according to the position information of the stationary points and the intersection points comprises: judging, according to the number of the stationary points and the relative position relationship between the intersection points and the stationary points, a distribution form of a function curve of each joint angle with respect to the arm angle relative to the corresponding angle limit line; determining, based on the distribution form, an arm angle sub-interval in which the corresponding joint angle is within the angle limit range; taking an intersection of the arm angle sub-intervals corresponding to all joints to obtain the feasible arm angle interval.

4. The SRS-configuration manipulator inverse kinematics control method of claim 3, wherein, The distribution form comprises: the function curve has two stationary points, and a maximum value point is lower than an upper limit of the joint angle and a minimum value point is higher than a lower limit of the joint angle; the function curve has two stationary points, and a maximum value point is higher than an upper limit of the joint angle and a minimum value point is lower than a lower limit of the joint angle; the function curve has no stationary point; the function curve has one stationary point, and a function value at the stationary point has a step change.

5. The SRS-configuration manipulator inverse kinematics control method of claim 4, wherein, When the function curve has one stationary point and the function value at the stationary point has a step change, the determination of the arm angle sub-interval in which the corresponding joint angle is within the angle limit range based on the distribution form comprises: calculating a left limit value and a right limit value of the function value at the stationary point; when the left limit value and the right limit value are both within the angle limit range of the corresponding joint, excluding the stationary point from the arm angle sub-interval. When the left limit value and / or the right limit value exceeds the angle limiting range, according to the intersection position of the function relationship and the angle limiting line, an arm angle interval satisfying the joint angle limiting requirement on both sides of the stationary point is selected, and the selected arm angle interval is included in the arm angle sub-interval.

6. The SRS-configuration manipulator inverse kinematics control method of claim 1, wherein, Before the step of establishing the function relationship, the method further comprises: setting a global configuration parameter, the global configuration parameter being used to specify a target angle symbol combination of joint two, joint four and joint six; based on the target angle symbol combination corresponding to the global configuration parameter, establishing a function relationship between each joint angle and arm angle, the function relationship corresponding to a group of joint angle solutions uniquely determined by the target angle symbol combination.

7. The SRS-configuration manipulator inverse kinematics control method of claim 1, wherein, The step of selecting a target arm angle according to the feasible arm angle interval comprises: based on the historical arm angle value of the last control period and the upper and lower boundaries of the feasible arm angle interval calculated at present, a target arm angle value of the current control period is calculated through a bounded cosine function mapping relationship.

8. The SRS-configuration manipulator inverse kinematics control method of claim 7, wherein, The bounded cosine function mapping relationship is: wherein ψ(t) is a target arm angle value of the current control period, ψ(t-1) is a historical arm angle value of the previous control period, ψ all_upper and ψ all_lower are respectively an upper boundary and a lower boundary of the feasible arm angle interval calculated in the current control period, K is a constant for controlling the strength of repulsion, and a is a constant for controlling the starting position of the repulsive reaction.

9. A SRS configuration robotic arm inverse kinematics control system, characterized in that, The SRS configuration mechanical arm inverse motion control method according to any one of claims 1-8 is used to control an SRS configuration mechanical arm, and the control system comprises: a stationary point acquisition module, which is used to establish a function relationship between each joint angle of a seven-degree-of-freedom mechanical arm and an arm angle, and determine a stationary point of each function relationship, the arm angle being defined as the rotation angle of an actual arm plane relative to a reference arm plane around a shoulder-to-wrist connecting line; an intersection point acquisition module, which is used to acquire an intersection point of each function relationship and a preset angle limiting line of the corresponding joint; an arm angle calculation module, which is used to calculate a feasible arm angle interval of the mechanical arm in the motion process according to the position information of the stationary point and the intersection point, the feasible arm angle interval being an arm angle value range in which all joint angles are within their corresponding angle limiting ranges; an instruction generation module, which is used to select a target arm angle according to the feasible arm angle interval, and generate joint angle control instructions of the mechanical arm based on the target arm angle.

10. An electronic device, comprising: comprise: at least one processor; and a memory connected with the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions are executed by the at least one processor to enable the at least one processor to execute the SRS configuration mechanical arm inverse motion control method according to any one of claims 1-8.

Images