Method and electronic equipment for adjusting arm angle of redundant mechanical arm based on numerical iteration

Abstract

The invention relates to a method for adjusting the arm angle of a redundant robot arm on the basis of numerical iteration, comprising the following steps: providing a key joint having a joint angle [theta]; the current arm angle is calculated through the current joint angle configuration of the key joints, and k is the number of iterations; iteratively calculating an arm angle difference between the initial guess value of the key joint and the target arm angle T by continuously adjusting the initial guess value of the key joint; the arm angle difference value is compared with a preset threshold value until the condition of-Tlt is met; ; and finally, all the joint angle configurations obtained when the conditions are met are regarded as the result obtained after the arm angle of the redundant mechanical arm is adjusted. The invention further relates to electronic equipment.

Description

TECHNICAL FIELD

[0001] The present application relates to a method for adjusting arm angles of a redundant robot arm based on numerical iteration, an electronic device, a computer readable storage medium and a computer program product. BACKGROUND

[0002] The conventional method for adjusting arm angles of a redundant robot arm is an analytical solution method. In the field of robot inverse kinematics, the analytical solution method has advantages such as high calculation efficiency and accurate solution, but its inherent defects limit its application value in complex scenarios. The main defects are as follows:

[0003] I. Limited applicability: strict geometric constraints, wherein the analytical solution only exists in mechanical arms with specific geometric structures (such as 6-DOF serial mechanical arms satisfying the Pieper criterion), which require joint axes to satisfy strict geometric relationships such as orthogonality / parallelism. For example, only when the first three joint axes of the mechanical arm intersect at a point (ball wrist structure), a closed solution can be obtained by separating the position / attitude variables; freedom redundancy failure, wherein for redundant mechanical arms (such as 7-DOF mechanical arms), the analytical solution cannot directly handle the freedom redundancy problem, and an additional optimization objective (such as joint limit avoidance) is required, resulting in a significant increase in calculation complexity.

[0004] II. Sensitivity to singularity: mathematical singularity leads to solution mutation, wherein when the mechanical arm is in a singular configuration (such as the wrist joint axes being collinear), the Jacobian matrix is rank-deficient, and the analytical solution corresponds to joint velocities tending to infinity. For example, when the PUMA 560 mechanical arm is fully extended at the wrist, the inverse solution calculation will exhibit numerical instability with a denominator tending to zero; feasibility rupture, wherein a small Cartesian space displacement near a singular point may require a dramatic jump in joint space, exceeding the actual motor driving capacity, resulting in failure of trajectory planning;

[0005] III. Sensitivity to modeling error accumulation: ideal assumptions deviate from reality, wherein the analytical solution relies on accurate DH parameter modeling, ignoring actual factors such as manufacturing tolerances and joint flexibility deformation. For example, when the link length has an error of 0.1 mm, the analytical solution may amplify the end positioning error to the millimeter level; closed-loop adjustment coupling difficulty, wherein the analytical solution as an open-loop feedforward cannot directly integrate sensor feedback (such as force / vision), making it difficult to cope with dynamic environmental disturbances.

[0006] IV. Real-time bottleneck: multiple solution screening time-consuming, wherein the analytical solution often produces multiple mathematical solutions (such as "elbow-up" and "elbow-down" configurations), which need to be screened for feasible solutions through collision detection, energy optimization, etc. The calculation amount increases exponentially with the degree of freedom, for example: a 6-DOF mechanical arm has up to 8 sets of solutions for inverse solution, and real-time screening requires tens of milliseconds of calculation resources.

[0007] V. Insufficient flexibility: unable to handle nonlinear constraints, where it is difficult to directly incorporate nonlinear constraints such as joint torque limits, obstacle avoidance, etc. into the analytical solution, and post-processing optimization is required, which destroys the closed-form solution; poor scalability, where for new variable topology mechanisms (such as modular reconfiguration of collaborative robots), the analytical expression needs to be re-derived, which has a long development cycle and low universality.

[0008] To overcome the above-mentioned defects, numerical iteration method (such as Newton-Raphson method) is often used in practical engineering, which has strong universality and is suitable for any degree of freedom and geometric structure; numerical iteration method has flexible constraint embedding, which can integrate hard / soft constraints such as joint limits and obstacle avoidance; numerical iteration method has high fault tolerance, which can handle singularity through pseudo-inverse of Jacobian matrix; numerical iteration method is suitable for online task changes (such as dynamic target tracking), combined with optimization objectives (such as minimum joint motion).

[0009] When facing complex configurations, redundant degrees of freedom or dynamic optimization constraints, such as service robots and medical manipulators, it is more appropriate to choose a numerical iteration method to adjust the arm angle of the redundant manipulator. SUMMARY

[0010] The technical problem to be solved by the present application is to provide a method for adjusting the arm angle of a redundant manipulator based on numerical iteration, which can avoid collision between the redundant manipulator and obstacles.

[0011] The technical problem is solved in one aspect by a method for adjusting the arm angle of a redundant manipulator based on numerical iteration, according to the present application, which comprises the following steps:

[0012] Setting a key joint with a joint angle ;

[0013] Calculating the current arm angle by the current joint angle of the key joint Configuring the current arm angle , wherein k is the iteration number;

[0014] The arm angle difference between the initial guess value of the key joint and the target arm angle T is iteratively calculated by continuously adjusting the initial guess value of the key joint;

[0015] The size of the arm angle difference and the preset threshold value ε is compared until the condition is met:

[0016] ;

[0017] Finally, the joint angle configuration obtained when the condition is met is considered as the result after adjusting the arm angle of the redundant manipulator.

[0018] According to the present invention, the method of adjusting the arm angles of redundant robotic arms based on numerical iteration is mainly used to control the operating posture of redundant robotic arms, thereby avoiding collisions and improving accuracy, especially in scenarios such as multi-arm surgical robots and automatic navigation of robotic arms. In the field of surgical robots, this method improves the flexibility and accuracy of surgical instrument movements by precisely adjusting the posture of the robotic arms, ensuring that multiple surgical arms can collaborate to complete complex surgical operations safely and efficiently, thereby reducing the risk of patient injury. The method comprehensively applies inverse kinematics algorithms, redundancy optimization methods, and trajectory planning algorithms to achieve high-precision control and dynamic real-time adjustment of the robotic arm posture during surgery, effectively adapting to complex surgical environments. During the automatic navigation process, the method introduces collision detection, obstacle avoidance path planning, and multi-arm collaborative control technology to coordinate the motion trajectories of multiple robotic arms, which can effectively avoid collision interference between robotic arms, optimize space utilization, and improve the reliability, intelligence, and operational efficiency of the robotic arm system.

[0019] In a further design of the method, it is provided that the initial guess value is set as the initial joint angle of the key joint .

[0020] The extended design of this method stipulates that the initial guess value of the key joint can be adjusted by Add an adjustment amount k Get the joint angle for the next iteration The setting of the adjustment amount is related to the threshold. If the adjustment amount is too large, the arm angle may be over-adjusted. If the adjustment amount is too small, the number of iterations k will increase significantly. The adjustment amount can be selected as a variable value, such as the current arm angle difference: The adjustment amount may also be selected as a fixed value, or other fixed values ​​to meet specific needs in actual operation scenarios.

[0021] During the adjustment of the initial guess values ​​for the key joints, a forward kinematics solver is used to obtain the end-point pose of the manipulator based on the current joint angle configuration, which is the angle of each joint detected by the sensors. An inverse kinematics solver is then used to recalculate the end-point pose of the manipulator to its original pose before adjustment.

[0022] In a further design of the method, it is provided that the method further comprises judging whether the time taken at the iteration number k is greater than or equal to a preset maximum iteration time Max_Time. As a supplement or alternative, if the time taken at the iteration number k is greater than or equal to the preset maximum iteration time Max_Time, a value of adjustment failure is returned. As a supplement or alternative, if the time taken at the iteration number k is less than the preset maximum iteration time Max_Time, the joint angle of the next iteration is obtained. Thus, a feasible solution (satisfying the actual demand) can be provided under limited resources and the control of the user over the calculation process is enhanced (convenient for debugging and optimization).

[0023] According to the present application, in order to control the direction of rotation of the elbow of the robot arm, it is further provided advantageously that if the initial arm angle difference is greater than or equal to 90 degrees, the arm angle is adjusted in multiple segments after the arm angle difference is calculated iteratively and before the arm angle difference is compared with the preset threshold. Adjusting the arm angle in multiple segments can strictly control the direction of rotation of the elbow and avoid collision, because the arm angle is too large and the hand elbow may be rotated in the opposite direction during the adjustment process. Therefore, for a target angle with an excessively large current arm angle difference, in order to ensure the safety of the movement of the robot arm, the present method adjusts the arm angle of the robot arm to the target angle in multiple segments, while also improving the precision and reducing the error.

[0024] It is particularly advantageous to avoid collision and improve the precision and reduce the error that if the initial arm angle difference is greater than 90 degrees and less than 180 degrees, the arm angle is adjusted in two segments. As a supplement or alternative, if the initial arm angle difference is greater than 180 degrees, the arm angle is adjusted in four segments.

[0025] In an extended design of the method of the present application, it is provided that the key joint is set as the first joint closest to the base of the robot, because the rotation of the first joint most directly affects the arm angle. Of course, the key joint can also be set as another joint of the same robot arm other than the first joint.

[0026] In an extended design of the method of the present application, it is provided that the threshold is preset to 1°. Of course, the threshold can also be preset to other values according to the demand to meet different application requirements. The smaller the threshold is set, the higher the precision of adjusting the arm angle of the redundant robot arm will be.

[0027] The technical problem is solved in another aspect by an electronic device, according to the present application, comprising a memory and a processor, wherein the processor is configured to execute program instructions stored on the memory to implement the method of adjusting the arm angle of the redundant robot arm based on numerical iteration.

[0028] The processor can also be referred to as a CPU (Central Processing Unit), which is an integrated circuit chip with signal processing capability. In the present disclosure, the processor 101 can be, but is not limited to, a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA).

[0029] The technical problem is solved in another aspect by a computer-readable storage medium having stored thereon program instructions which, when executed by a processor, implement the method for adjusting the elbow posture of a redundant robot arm based on numerical iteration.

[0030] In the present disclosure, the computer-readable storage medium can be, but is not limited to, a U disk, an SD card, a PD optical drive, a mobile hard disk, a large-capacity floppy disk drive, a flash memory, a multimedia memory card, a server, etc.

[0031] The processes or methods depicted in the flowcharts or otherwise described herein can be understood as representing code modules, segments, or portions of code which include one or more executable instructions for implementing specific logic functions (or steps) in the processes. The flow steps can also be implemented in an order other than that shown or discussed, without departing from the basic features of the present disclosure.

[0032] Within the scope of the present disclosure, the computer-readable medium can be any device that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device. The computer-readable medium can be, but is not limited to, an electrical connection having one or more wires (electronic devices), a portable computer diskette (magnetic devices), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM).

[0033] The technical problem is solved in another aspect by a computer program product, according to the present application, which comprises a command set for executing the method for adjusting the arm angle of a redundant robot arm based on numerical iteration. BRIEF DESCRIPTION OF DRAWINGS

[0034] The features and advantages of the present application will be further illustrated in the following description in conjunction with the accompanying drawings, in which:

[0035] Figure 1 is a collision scenario diagram applying the method for adjusting the arm angle of a redundant robot arm based on numerical iteration of the present disclosure;

[0036] Figure 2A is a schematic diagram of a non-limiting embodiment of a redundant robot arm designed as a seven-degree-of-freedom robot arm according to the present application;

[0037] Figure 2B is a schematic diagram of arm angles of a redundant robot arm according to the present application;

[0038] Figure 3 is a flow chart of one non-limiting embodiment of the method of adjusting arm angles of a redundant robot arm based on numerical iteration according to the present application;

[0039] Figure 4 is a flow chart of an extended design of the method according to the present application. DETAILED DESCRIPTION

[0040] The terms "first", "second", etc. are only used to distinguish different technical features, such as components, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. In the description of the present application, the meaning of "a plurality of" is at least two, such as two, three, etc., unless otherwise explicitly specified.

[0041] Figure 1 is a diagram of an exemplary collision scenario in which the method of adjusting arm angles of a redundant robot arm based on numerical iteration according to the present application is applied. For example, when one of the robot arms collides with the other two robot arms assembled with the robot, the arm angle of the robot arm can be adjusted by the method of adjusting arm angles of a redundant robot arm based on numerical iteration, so that the robot arm avoids the obstacle plane shown.

[0042] As shown in Figure 1 , the highest point of the elbow of the robot arm has collided with the obstacle plane, and the end pose of the robot arm needs to be maintained, the arm angle of the robot arm can be adjusted to avoid the obstacle plane.

[0043] The method according to the present application can also be applied in other scenarios, such as: in the assembly process of electronic products, automobile parts, etc., the control method of the arm angle can improve the support for the robot arm to avoid other workpieces and obstacles in the assembly process.

[0044] Figure 2A is a schematic diagram of one non-limiting embodiment of a redundant robot arm designed as a seven-degree-of-freedom robot arm according to the present application, which has a total of seven joints, each of which can rotate around a direction, wherein the first joint is arranged at one end of the base B, and the other end is coupled with the second joint, followed by the third joint, the fourth joint, the fifth joint, the sixth joint and the seventh joint, which is located at the end of the robot arm, the angles of the joints are represented by , the angle of the first joint is 1, the angle of the second joint is 2, the angle of the second joint is 3, the angle of the second joint is 4, the angle of the second joint is 5, the angle of the second joint is 6, the angle of the second joint is 7.

[0045] Referring to the structure of the human body, the third joint is defined as the shoulder S of the mechanical arm, the fourth joint is defined as the elbow E of the mechanical arm, the sixth joint is defined as the wrist W of the mechanical arm, and the seventh joint is defined as the wrist of the mechanical arm and the tool shaft T.

[0046] Figure 2B is a schematic diagram of the arm angle of the redundant mechanical arm according to the present application, in which the shoulder S of the mechanical arm, the wrist W of the mechanical arm and the elbow E of the mechanical arm are respectively represented by a point in a simplified manner, the plane formed by the points E, W and S is called the arm plane, the angle of rotation of the arm plane around the line connecting the points S and W is the arm angle Ψ, and the arm plane after rotating an arm angle Ψ makes the elbow E of the mechanical arm located in the plane defined by the dashed line and represented by E'.

[0047] The method for adjusting the arm angle of the redundant mechanical arm based on numerical iteration according to the present application can not only be used in surgical robots to bypass other mechanical arms and medical staff, etc., but also be used in industrial robots to avoid other workpieces and obstacles in the assembly process of electronic products, automobile parts, etc.

[0048] Figure 3 is a flowchart of one non-limiting embodiment of the method for adjusting the arm angle of the redundant mechanical arm based on numerical iteration, and the numerical iteration method according to the present application is used to solve the final joint angle configuration.

[0049] The method for adjusting the arm angle of the redundant mechanical arm based on numerical iteration according to the present application comprises:

[0050] Step S1: setting initial values: initial joint angle (denoted by ), target arm angle (denoted by T), error threshold of arm angle (denoted by ), maximum iteration time (denoted by Max_Time), iteration number (denoted by k), and iteration time (denoted by t).

[0051] The joint angle can be any one of the first joint, the second joint, the third joint, the fourth joint, the fifth joint, the sixth joint and the seventh joint of the seven-degree-of-freedom mechanical arm according to the present application, and as an example, the joint angle is given as the first joint angle in one non-limiting embodiment of the method according to the present application.

[0052] For example, the target arm angle T can be set to 30°, and of course it can also be set to any other value according to the application environment.

[0053] For example, the arm angle threshold can be set to 1°.

[0054] The maximum iteration time Max_Time is selected to be no more than 3 seconds in consideration of the efficiency of the surgical procedure.

[0055] Step S2: calculating the current arm angle difference e according to the formula k The function represents: for the joint angle , for example the first joint angle (other fixed joint angles are also feasible), the arm angle calculated in the solution obtained by the inverse kinematics solver, also called the elbow angle, where is the joint angle at the th iteration, = +e k The current arm angle represents the arm angle at the current path point, which corresponds to the current joint angle configuration, which refers to a parameter set composed of the angle values of multiple joints, used to describe the specific posture or position of the redundant robot arm of the present disclosure at the current time.

[0056] Step S3: determining whether the current arm angle difference e k is greater than the error threshold value of the arm angle .

[0057] If the result of the determination in S3 is "yes", i.e. the current arm angle difference e k is greater than the arm angle threshold value , then step S4-1 is executed: determining whether the time t k elapsed at the iteration number k is greater than or equal to the maximum iteration time Max_Time.

[0058] If the result of the determination in S4-1 is "yes", i.e. t k ≥ Max_Time, then step S5-1 is executed: outputting the result of the failure of the arm angle adjustment, for example outputting a Boolean value indicating the failure of the arm angle adjustment.

[0059] If the result of the determination in S4-1 is "no", i.e. t k < Max_Time, then step S5-2 is executed: modifying the joint angle in the direction of the arm angle, i.e. updating the joint angle according to the formula = +e k In a non-limiting embodiment of the present disclosure, the joint angle is the first joint angle, and in the process of adjusting the initial guess value of the key joint, the end pose of the robot arm is first obtained based on the current joint angle configuration by the forward kinematics solver, where the current joint angle configuration is the joint angle of each joint detected by the sensor, and then the end pose of the robot arm is recalculated as the end pose before adjustment by the inverse kinematics solver.

[0060] Step S6 is performed: the inverse kinematics solver calculates the joint angle configuration that keeps the end-effector pose unchanged, and then step S7 is performed: iteration number k = k + 1 and time t = t + single calculation time, where the single calculation time can be defined as the total time taken by the program to complete three tasks (i.e., to calculate the end-effector pose corresponding to the current joint angle configuration by forward kinematics, to solve the joint angle configuration required to restore the end-effector pose to the state before adjustment by inverse kinematics, and to calculate the current arm angle based on the joint angle configuration) through the computer after the key joint angle is changed.

[0061] Then, return to step S2: calculate the current arm angle difference e k .

[0062] If the result of the judgment in S3 is “No”, i.e., the current arm angle difference e k is less than or equal to the arm angle threshold , step S4-2 is performed: output the final joint angle configuration as the result of successful arm angle adjustment, which is considered to achieve the solution after adjusting the arm angle.

[0063] Figure 4 is a flowchart of an extended design of the method of the present disclosure. To avoid repeated description, only the parts different from Figure 3 will be described below, compared with Figure 3 , Figure 4 In the method of Figure 4 , a step S1-2 (represented by a dashed box in ) is added between steps S1 and S2: whether to perform segmented adjustment of the arm angle of the robot arm to the target angle is determined according to the size of the initial arm angle difference e0, so as to ensure the safety of the movement of the robot arm. By adjusting the arm angle of the robot arm to the target angle T in segments, the rotation direction of the elbow of the robot arm can be controlled.

[0064] If the initial arm angle difference e0 is less than 90°, step S2 is directly performed; if the initial arm angle difference e0 is not less than 90°, the arm angle of the robot arm is adjusted in segments, and then step S2 is entered. Specifically, the segmented adjustment can be performed as follows: when the initial arm angle difference e0 is greater than 180°, the arm angle of the robot arm is adjusted in four segments, with each adjustment being 0.25e0; if the initial arm angle difference e0 is less than 180° but greater than 90°, the arm angle of the robot arm is adjusted in two segments, with each adjustment being 0.5e0. By adjusting the initial arm angle difference e0 in two cases, i.e., the case greater than 180° and the case less than 180° but greater than 90°, it can be ensured that the robot arm adjusts in a predetermined direction, thereby avoiding possible collision with other robot arms or other components.

[0065] The above description is merely an implementation method of the present application and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the description and drawings of this application, or directly or indirectly applied in other related technical fields, are also included in the protection scope of this disclosure.

Claims

1. A method for adjusting the arm angle of a redundant manipulator based on numerical iteration, characterized in that: The method comprises the following steps: Set up key joints, which have joint angles ; By the current joint angle of the key joint Configure and calculate the current arm angle , where k is the number of iterations; The arm angle difference between the target arm angle T and the initial guess value of the key joint is iteratively calculated by continuously adjusting the initial guess value of the key joint; Compare the arm angle difference with the preset threshold ϵ until the condition is met: ； Finally, the joint angle configurations obtained when the conditions are met are regarded as the results after adjusting the redundant robotic arm angles.

2. The method according to claim 1, characterized in that Set the initial guess value as the initial joint angle of the key joint .

3. The method according to claim 1, characterized in that The way to adjust the initial guess value of the key joint is: Add an adjustment amount k Get the joint angle for the next iteration .

4. The method according to any one of claims 1 to 3, characterized in that The end pose of the robot arm is obtained based on the current joint angle configuration using a forward kinematics solver.

5. The method according to claim 4, characterized in that The end position of the robot arm is recalculated as the end position before adjustment through an inverse kinematics solver.

6. The method according to claim 4, characterized in that The current joint angle configuration is the joint angle of each joint detected by the sensor.

7. The method according to claim 3, characterized in that The adjustment amount is set to 5°.

8. The method according to any one of claims 1 to 4, characterized in that The method further includes determining whether the time elapsed during the iteration number k is greater than or equal to a preset maximum iteration time Max_Time.

9. The method according to claim 8, characterized in that If the time elapsed at iteration number k is greater than or equal to the preset maximum iteration time, the adjustment failure value is returned.

10. The method according to claim 8, characterized in that If the time elapsed at iteration number k is less than the preset maximum iteration time Max_Time, the joint angle of the next iteration is obtained.

11. The method according to any one of claims 1 to 3, characterized in that If the initial arm angle difference is greater than or equal to 90 degrees, the arm angle is adjusted in multiple stages after iteratively calculating the arm angle difference and before comparing the arm angle difference with the preset threshold.

12. The method according to claim 11, characterized in that If the initial arm angle difference is greater than 90 degrees and less than 180 degrees, the arm angle is adjusted in two stages.

13. The method according to claim 11, characterized in that If the initial arm angle difference is greater than 180 degrees, the arm angle is adjusted in four stages.

14. The method according to any one of claims 1 to 3, characterized in that The key joint is set as the first joint closest to the robot base.

15. The method according to claim 8, characterized in that The maximum iteration time Max_Time is no more than 3 seconds.

16. An electronic device, characterized in that: It comprises a memory and a processor, wherein the processor is used to execute program instructions stored in the memory to implement the method for adjusting the arm angle of a redundant robotic arm based on numerical iteration as described in one of claims 1-15.

17. A computer-readable storage medium, characterized in that Program instructions are stored thereon, and when the program instructions are executed by a processor, the method for adjusting the arm angle of a redundant robotic arm based on numerical iteration as described in one of claims 1 to 15 can be implemented.

18. A computer program product, characterized in that It includes a command set for executing the method for adjusting the arm angle of a redundant robotic arm based on numerical iteration as described in one of claims 1 to 15.

Images